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ABSTRACT 

The  performance  of  a  modified  linear  Kalman  filter  with 
adaptation  is  compared  with  that  of  a  common  adaptive 
alpha-beta  filter  for  state  estimation  of  a  pilot 
controlled,  ground  directed  bombing  system.  Of  particular 
concern  is  the  accuracy  and  response  of  the  alternative 
filters  when  the  aircraft  conducts  random  maneuvers  in  the 
vicinity  of  the  target.  The  desirablity  of  including 
deterministic  forcing  in  the  filter  model  is  discussed  and  a 
technigue  utilizing  an  adaptive  Kalman  identifier  to 
establish  the  pilot  response  to  ground  control  heading 
commands   is    presented. 
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I.       INTRODUCTION 

The  ground  directed  bombing  system  simulated  is 
conceptually  similar  to  the  OSMC  AN/TPB-1D  produced  by 
Sierra      Research      Corporation.  This        system      tracks      the 

tactical  aircraft  with  a  conical  scan  radar,  filters  the 
noisy  radar  data,  calculates  heading  commands  based  on  the 
smoothed  trajectory,  and  transmits  this  guidance  information 
to  the  pilot  via  the  Tacan  navigation  system  located  in  the 
cockpit.  This  heading  information  directs  the  pilot  to  fly 
the  aircraft  so  that  its  ground  track  vector  passes  through 
the      calculated      ordnance      release      point.  Audio      signals 

transmitted   to   the    pilct  designate    the    bomb    release   time. 

In  an  operational  environment  such  a  system  would 
possibly  be  required  to  track  and  guide  aircraft  conducting 
significant      maneuvers        enrouts      zo      the        target.  These 

maneuvers  would  most  likely  be  dictated  by  tactical  doctrine 
or    by    the   threat    environment. 

With  this  operational  model  in  mind  an  appropriate 
concern  is  the  capability  of  a  ground  directed  bombing 
system  to  track  and  guide  an  aircraft  exhibiting  random 
maneuvers      until    moments      prior      to      bomb  release.  It      is 

obvious   that    the    smoothing    filter      should    be    able   to    respond 


to  maneuvers,  yet  settle  quickly  to  an  accurate  solution  as 
the      pilot        steadies      the      aircraft.  These      conflicting 

requirements  are  investigated  utilizing  both  alpha-beta  and 
Kalman   filtering   techniques. 

Similar     filtering        techniques      were      utilized        for     a 
simulation      of   the      US MC  AN/TPQ-27,    [ i ].  However    in      that 

ground      directed      bombing      system,  control      signals      were 

directly  coupled  to  the  aircraft  aerodynamic  controls,  thus 
eliminating  the  uncertainty  of  pilot  response  in  the  control 
loop.  In   that,      study  significant      improvements    in      filter 

response  and  accuracy  wer?  realized  by  including 
deterministic  forcing  autopilot  commands  in  the  state 
estimation    via    the   Kalman    filters. 
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II.       GROUND    DIRECTED    BOMBING    SYSTEM     (GDBS)     SIMULATION 

A.       COORDINATE    SYSTEMS 

A  Cartesian  coordinate  system  was  chosen  for  the 
aircraft  dynamics  model  and  a  radar  centered  inertial 
reference   frame.  In    this   reference      system   the      y-axis   is 

directed  toward  true  North  and  the  x-axis  toward  the  east. 
The  z-axis  is  directed  away  from  the  center  of  the  earth. 
All  radar  measurements  of  aircraft  position,  however,  are 
obtained  in  spherical  polar  coordinates,  i.e.  slant  range 
(R)  ,  azimith  angle  from  true  North  (A)  ,  and  elevation  angle 
(E)  from  the  horizontal.  Figure  2.1  shows  these  coordinate 
systems  and  their  transformation  relationship.  Wind  is 
modeled  with  a  constant  velocity  in  the  x-y  plane  with  no 
vertical   component. 

Curvature  of  the  earth  and  the  fact  that  pilot  heading 
information  is  oriented  to  magnetic  north,  were  not  taken 
into  account  in  the  simulation.  Also  bomb  ballistics  and 
therefore  corioiis  forces  were  net  included  in  the  model. 
The  aircraft  is  simply  directed  to  a  release  point  in  space, 
which  in  a  full  simulation  would  be  derived  from  the 
projected  bomb  trajectory  , ballistic  winds,  corioiis  forces, 
and   a    number   of    other    factors,    all    of    which    are    important   to 
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x  =  R  cos (E)  sin (A) 
y  =  R  cos(E)  cos(A) 
2  =  R  sin  (E) 


Fig.  2.1.   Model  Simulation  Coordinate  Systems 
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the  problem  as  a  whole  but  are  not  necessarily  germane  to 
the  objective  of  evaluating  the  response  and  accuracy  of 
alternative  state  estimating  filters  for  a  goal  oriented 
maneuvering  bomber.  Thus  the  simulation  has  been  simplified 
appropriately. 

B.       AIRCRAFT    DYNAMICS     MODEL 

The  dynamic  model  of  the  aircraft  for  the  purpose  of 
simulation  was  assumed  to  be  a  free  inertial  (1/s  )  plant 
since  the  bombing  profile  dictates  a  constant  aircraft 
airspeed.  The  discrate  realization  of  this  plant  is  shown 
in    (2.1). 


X(k  +  1)    =    <£(k+1/k)X(k)     +    A(k+1/k)U(k) 


(2.1) 


where 


0(k+1A) 


1  T    0  0    0  0 

0  10  0    0  0 

0  0     1  T    0  0 

0  0    0  10  0 

0  0    0  0    1  T 

0  0    0  0    0  1 


(2.2) 
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A(k+1A) 


T^2 

0 

0 

T 

0 

0 

0 

2 

T72 

0 

0 

T 

0 

0 

0 

2 

T?2 

0 

0 

T 

(2.3) 


In  the  controlled  mode  the  aircraft  model  simulates  the 
pilot  responding  to  heading  inputs  transmitted  from  the 
radar  site  to  the  TACAN  navigation  system  in  the  cockpit. 
The  model  is  driven  by  a  pilot/aircraft  control  function 
similar  to  that  developed  in  [  1 ]  and  shown  in  Figure  2.2. 
The  input  is  bearing  to  the  target  and  the  output  is  a  bank 
angle  which  generates  a  heading  rate  that  can  be  transformed 
into  x-y  accelaration  components  for  entry  into  the  dynamic 
model.  It  is  assumed  that  in  the  controlled  mode  heading 
changes  are  made  with  coordinated  turns  performed  by  the 
pilot  in  response  to  heading  commands  displayed  by  the 
TACAN.  The  pilot /aircraft  controller  induced  x-y 
accelerations  are  depicted  in  Figure  2.3  and  summarized  by 
(2.4)  and  (2.5)  below. 


x(k)  =  V(k)  cos(i//(k)  )  i//(k) 


(2.4) 


y(k)  =-7(k)  sin(i//(k)  )  t£(k) 


(2.5) 
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Controller 


-3<f 


ft, 


^   A/C  Roll 
Response 


9 


A/C 

Dynamics 


dt 


Compute 
Heading  Err. 


"TFT 


Bearing  to  Tgt. 


State  Est. 
Filter 


<■ 


±. 


Measurement 
Process 


Fig.  2.2.   Pilot/ Aircraft  Controller  Configuration 


i//(k)  and  7  (k)  are  aircraft  heading  and  velocity  respectively 
at  time  k,  and  t/>(k)  is  the  heading  rate  which  is  derived  in 
(2.6)  through  (2.9)  from  the  free-body  diagram  shown  in 
Figure  2.4.   The  aircraft  weight  is  shown  in  (2.6)  below. 


W  =  mg  =  L  cos0 


(2.6) 
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A 


X 


Fig.    2.3.      Pilot  Induced    X-Y    Accelerations 


Equation  (2.7)  depicts  the  centripetal  force  generated  in  a 
turn,  where  R  is  the  turn  radius,  L  is  lift,  V  is  velocity, 
and     ^  is   the   bank  angle. 


?  =    <nV/R    =    L    sin0 


(2.7) 


Eut    V/R  =  \p       ,  the    .urn    rate,    so 


P  =    nV^  =    L    sin  d 


(2.8) 
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.L    sin0 


Fig.    2.4.      Coordinated  Turn    Free-Body    Diagram 


Dividing  (2.8)  by  (2.6)  and  rearranging  terms  yields  (2.9), 
which  defines  if  f  the  aircraft  turn  rate,  as  a  function  of 
aircraft   bank   angled,    and    velocity    V. 


if  =    g/V   tan0 


(2.9) 


From  [1]  the  aircraft  roll  response  is  assumed  to  be  of 
the  form  shown  in  (2.10)  where  T  is  the  roll  response  time 
constant. 


17 


e 
el 


=  V(sr  ♦  1) 


(2.  10) 


No  effort  has   been  made  to  specifically  model  pilot  delays 
or  response  to  visual  inputs  from  the  TACAN. 

In  the  maneuvering  mode  the   maneuver  model  described  in 
[2]  was  used  and  is  shown  in  Figures  2.5  and  2.6  . 


a 


P(a) 


AP1 


9> 


-A 


Fig-  2.5.   Acceleration  Probability  Density  Model 


This  was  simulated  for  uncontrolled  random  flight  since  the 
aircraft  is  assumed  to  typically  move  at  a  constant  velocity 
with  turns,  evasive  maneuvers,  and  air  turbulence 
interpreted  as  perturbations  upon  the  constant  velocity 
trajectory.    These  maneuver   perturbations  or  accelerations 
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Fig.    2.6.       Model    Acceleration    Correlation   Function 


can   be   specified    by    a    magnitude,    with    probability    P(a)       from 
(2.11),      and   duration    of  H{a)       from    (2.12),      the   correlation 

function  of  aircraf-   acceleration. 


P(a)    =    (1-(2PX     *    P    )  )/2A 


(2.11) 


R(a)     =   B    exp  (-t|an 


(2.12) 


The      acceleration      A  in      (2.11)  is      the   maximum      that      can 

reasonably      be      expected      from  the      pilot/aircraft      in      the 

environment   described.         P-     is  that    probabili-y   assigned   to 

the   maximum  acceleration  ±A,    P.  is    that    probability    assigned 
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to  no  maneuver,  and  the  assumed  probabilitity  distribution 
between  these  values  is  uniform  with  amplitude  P  (a)  . 
Equation  (2.12)  is  the  correlation  function  which  yields  an 
acceleration  time  duration,  R  (a)  ,  which  is  based  on  the 
magnitude     of   the     acceleration    |a|.  M   and      t   are      simply 

correlation  factors  which  determine  how  the  H (a)  varies  over 
the   range      of    possible    acceleration   amplitudes.  From    this 

model  one  can  see  how  the  duration  of  a  high  G  maneuver  for 
threat  avoidance  would  be  considerably  less  than  for  a  low  G 
clearing   turn. 

9 

C.   GDBS  MODEL  IMPLEMENTATION 

The  GDBS  model  was  simulated  on  an  IBM  370  in  single 
precision  Fortran.  Figure  2.7  shows  the  basic  flow  diagram 
for  the  computer  program,  which  implements  this  simulation 
model.  The  module  labeled  'State  Estimation  Filter' 
represents  those  filter  algorithms  discussed  in  the  next 
chapter. 
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Fig.  2.7.   GDBS  Model  Flow  Diagrai 
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III.       AIRCRAFT    STATE    ESTIMATION 

A.  BACKGROUND 

A  great  deal  has  been  written  on  the  theory  and 
application      of   estimation      filters.  In   particular,         [2] 

provides  a  good  overview  of  several  such  filters,  including 
the  alpha-beta  and  Kalman  filters,  and  compares  their 
relative  performance  not  only  in  terms  of  accuracy  and  in 
response,  but  also  in  terms  of  computer  implementation  costs 
in   computation  time   and  storage   overhead. 

The  general  conclusion  is  that  the  Kalman  filter 
out-performs  an  alpha-beta  filter  of  comparable  order  by 
about  2  to  1.  However,  the  cost  for  such  perfcrmanca  is 
increased  computer  computation  time  and  memory,  of  the  same 
relative   magnitude. 

B.  GENERAL    DESCRIPTION    OF    THE    ALPHA-BETA    FILTER 

The  basic  theory  of  the  alpha-beta  filter  is  derived 
from  minimizing  the  mean  square  error  of  the  filtered 
states.  A  classic  analysis  of  the  alpha-beta  filter  is 
provided      by  [3].  The      filter        recursive      equations      are 

summarized    below. 

x (k/k-1)=x  (k-1/k-1)     +    T    x(k-1/k-1)  (3.1) 
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x(k/k)    =  x  (kA-1)    ♦     a(z(k)    -  x(k/k-1)  (3-2) 


x(k/k)    =  x  (kA-1)    ♦  £/T    (z(k)    -    x(k/k-1))       (3.3) 

x(k/k-1)  is  the  predicted  position,  X  (k/k)  is  the  updated 
position,    x  (k/k)       is  the   updated    velocity,      and    z  (k)       is    rhe 

noise  contaminated  measurement  of  position  at  the  k-th 
interval.  T  is  the  sample  rate  of  the  measurement  process, 
a  and  0  are  usually  fixed  real  constants.  As  pointed  out  in 
[  4  ]  these  alpha-beta  equations  are  analogous  to  a  steady 
state      Kalman    filter.  For      typical      parameter    values      the 

alpha-beta  filter  is  simply  low  pass  with  a  heavily  damped 
time  response.  Thus    the    filter      eliminates   not      only    most 

high  frequency  measurement  and  process  noise,  but  also  most 
maneuver  energy   from   the    state   estimate. 

C.       GENERAL    DESCRIPTION    OF    THE    KALMAN    FILTER 

The  Kalman  filter  generates  a  minimum  variance  estimate 
cf  the  plant  (aircraft)  state  vector  when  the  measurement  and 
plant  process  noise  statistics  are  known  and  conform  to  the 
criteria   shown   below. 

E(V(k)7(  j)T)     =    R(k)  5(k,j)  (3.4) 
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E(A(W(k)  W(j)T)AT)    =    Q(M5(k,j)  (3.5) 


E(V(k)W(j)T)     =   0  for   all  k,j  (3.6) 


,  0      k*j 
where  5(k,j)     *      <  (3.7) 

1      k=j 


A      linear      time- invariant      systam    is      assumed,         as      in      the 
discrete   model      representation   shown      in  Figure    3.1.  X  (k) 

represents  the  (n  x  1)  state  vector,  Z (k)  the  (m  x  1)  output 
vector,  0(k)  the  state  transition  matrix,  H  (k)  *he  (m  x  n) 
observation  matrix,  W(k)  state  excitation  or  process  noise, 
and  V  (k)    the    measurement  noise. 

The     Kalman      filter   recursion      algorithm      is      summarized 
below. 


X  (k+  1)    =    0(k)X(k)     «•      A(k)W(k)  (3.8) 


Z(k)     =   H  (k)  X  (k)     +    V(k)  (3.9) 


X(k/k-1)     =    <£(k,k-1)X(k-1/k-1)     +    A(k)U(k-1)        (3.10) 
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X(k) 
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Delay 
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■y 

X(k-l) 

Fig.  3.1.   Dynamic  System  Model  and  Discrete  Kalman  Filt 


er 


P(k/k-1)  =  <£(k,k-1)P(k-1/k-1)  0<k,k-1)  +  Q  (k)   (3.11) 


-1 


G(k)  =  P  (k/k-1  )H(k)  |_H(k)  ?  (k/k-1)H(k)T  +  B  (k)J    (3.12) 
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$(k/k)    =    X(K/K-1)  +  G  (k)  [Z  (k)    -    H(k)  X  (k/k-l]l  (3.13) 


P(k/k)    =    (I  -    G(k)  H(k) )  P(k/k-1)  (3.14) 

X(k/k-1)    denotes   the   estimate  of    the   state   vector   X(k)    based 
on     (k-1)    measurements,    Z(1)fZ(2), Z(k-1). 


D.       FILTER    ORDER    CONSIDERATIONS 

The  filter  order  is  chosen  to  match  as  closely  as 
possible  the  expected  plant  dynamics  of  the  system  being 
modeled.  A  first  order  filter  would  be  expected  to  estimate 
a  constant  velocity  trajectory  effectively  and  a  second 
order  filter  would  accordingly  observe  a  trajectory 
exhibiting  constant  acceleration.  The  order  is  used  here  in 
the  mathematical  sense  and  refers  to  the  order  of  the 
differential   equation    that    defines   the   filter. 

Since  the  aircraft  is  known  to  be  constrained  to  a 
constant  velocity  profile  as  it  approaches  the  release 
point,  it  would  seem  reasonable  to  select  a  first  order 
filter  for  modeling.  The  aircraft  dynamics  are  anticipated 
to  depart  from  the  first  order  model  enroute  to  the  target 
thus  creating  transient  errors  which  must  be  dealt  with  by 
filter   adaptation.         The   alternative    to      this    strategy   is   to 
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increase  the  order  of  the  filter  to  observe  these  high 
energy      maneuvers     for      state     estimation.  However,        the 

settling  time  of  a  first  order  filter  is  generally  less  -han 
that  of  a  second  order  filter  as  discussed  in[  5  ]  and 
graphically  illustrated  by  the  first  and  second  order  Kalraan 
gain   schedules   shown  in   Figures    3.2   and   3.3   respectively. 
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Fig.  3.2.   First  Order  Kalman  Gain  Schedule 
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Fig.    3.3.      Second    Order   Kalman    Filter   Gain   Schedule 


E.       FILTER    ADAPTATION 
1 .       Background 

No  matter  what  the  order  or  the  complexity  of  the 
filter  type  selected,  it  cannot  be  expected  to  fully  model 
the  aircraft  dynamics  and  process  noise  covariance.  The 
model    is      based    on     a    linear      time-invariant    system      and    the 
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process  noise  is  assumed  to  be  stationary,  white,  and 
Gaussian.  During  portions  of  the  flight,  paricularly  as  the 
aircraft  approaches  the  release  point,  the  system  dynamics 
are  expected  to  approximate  very  closely  the  assumed  model. 
However,,  during  othar  portions  of  the  flight  the  aircraft 
dynamics  are  anticipated  to  depart  significantly  from  the 
filter  model.  Certainly  the  pilot  should  not  be  constrained 
to  behave  in  a  manner  consistent  with  the  model,  if  the 
environment   dictates   otherwise. 

Since  the  pilot/aircraft  dynamics  are  not  fully 
modeled,  the  subopt imal  filter  that  results  might  be 
expected  to  diverge,  e.g.  the  ;rror  covariance  generated  by 
the  filter  and  the  actual  error  covariance  become 
inconsistent.  The  desire  is  for  the  filter  to  transition 
smcothly  between  accurate  estimations,  when  the  aircraft 
dynamics  conform  to  those  assumed  for  the  model,  and  less 
accurate  estimations,  when  the  aircraft  dynamics  do  not 
agree    with      the    model.  An   adaptive      filter   realizes      this 

smooth  transition  by  adjusting  filter  parameters  to  vary  the 
filter  bandwidth  to  allow  a  more  consistant  match  between 
the   calculated   and    actual    filter    error   covariar.ces. 
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In  the  case  of  Kalman  filter  adaptation,  the 
calculated  error  co  variance  becomes  a  function  of  the 
measured  data  indirectly  by  making  the  filter  parameters 
dependent   on      the  observed      aircraft   motion.  The   adaptive 

techniques  for  the  alpha-beta  filter  are  similar  in  concept. 
In  either  case  the  adaptive  process  is  conceptually  straight 
forward;  first  divergence  is  detected,  then  the  filter 
parameters   are   modified. 

In  the  case  of  a  ground  directed  bombing  system  of 
the  type  considered  here,  the  aircraft's  behavior  could 
depart  from  the  filter  model  in  a  random  fashion  when  the 
pilot  maneuvers  in  response  to  a  random  event  in  the 
environment,  or  determ inistically  when  he  responds  to  target 
tearing  inputs  from  the  ground  radar.  These  two  situations 
may  be  treated  separately  or  together  for  the  purpose  of 
filter  adaptation.  To  treat  them  separately,  as  random  and 
deterministic         processes,  requires      knowledge        of        the 

pilot/aircraft   response     to  target    bearing   inputs.  In    the 

case  of  the  ground  directed  bombing  system  described  in  [1] 
this  transfer  function  was  known  quite  accurately  since  the 
input  signals  from  the  ground  radar  were  directly  coupled  to 
the  aircraft    aerodynamic  controls,    with   the    uncertainties   of 
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pilot  response  isolated  from  the  contr.ol  loop-  With  that 
information  the  deterministic  forcing  could  simply  be 
integrated  into  the  filter  model.  Unfortunately  such  is  not 
the  case  for  this  simulation.  A  unigue  approach  to  this 
problem  will  be  discussed  later.  The  alternative  approach 
is  to  consider  both  processes  to  be  random  and  proceed  from 
that  assumption. 

2 .   Innovations  Statistics  and  Maneuver  Detection 

As  descibed  in  [5]  the  innovations  or  residual 
seguence  of  a  filter  can  be  observed  in  order  to  detect  a 
bias  that  would  indicate  divergence  of  the  stare  estimate 
from  the  true  state.   This  is  given  by 

l/(k/k-1)  =  Z(k)  -  B  (k)  X(k/k-1)  (3.15) 

By  substituting  for  Z  ( k)  from  (3.9),  the  measurement  model, 
we   see   that 

l/(k/k-1)    =    v(k)     -   H(k)€(k/k-1)  (3.16) 

where 

€(k/k-1)     =    X(k/k-1)     -    X(k)  (3.17) 

Taking   expected    values    ,    we   find    that 

E(j/(k/k-  1))     =    0  (3.18) 
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B(y(k/k-1)  i/T(k/k-  1) )    =   R  (k)    ♦    H  (k)  P  (k/k-1)  H  T(k)       (3.19) 


Thus  by  referring  to  the  model  statistics  for  R(k)  and 
P(k/k-1),  it  becomes  clear  that  when  the  system  conforms  to 
the  system  model,  i.e.  the  filter  is  operating  optimally, 
the  innovation  sequence  should  be  zero-mean  Gaussian  with 
variance 


(J2  =     a2(k)     ♦   P  (k/k-1) 


(3.20) 


One  appraoch  to  adaptation  considers  the  correlation 
of  the  innovation  sequence,  where  the  autocovariance 


Qp(i)    =  E(i/(k/k-1)  j/(k-i/k-1-i)T) 


(3.21) 


should  vanish  for  i*0.  Based  on  these  statistics,  maneuver 
detection  can  be  realized  by  observing  the  signs  of  the 
innovation  sequence.  The  probability  that  a  given  sequence 
is    either   positive   or    negative   is 


P(  0  >l>  >  0  )    =    .5 
1-N 


Another   approach    utilizes    (3.23) 


N 


(3.22) 


vk) 


>    c  <r     (k) 


(3  .23) 


to    declare      a    maneuver    when     V*.      exceeds   a      specified   value, 
usually  two   or   three  standard    deviations   of    <j_    . 


32 


Subsequent     to        maneuver      detection,  the      filter 

parameters  must  be  modified  to  correct  filter  divergence. 
Reference  [51  summarizes  numerous  techniques,  some  being 
guite      complex      and   computation      intensive.  The      strategy 

chosen  for  this  simulation  was  simply  to  reset  the  error 
covariance  in  response  to  a  detected  maneuver.  In  the 
situation  of  a  ground  directed  bombing  system,  the  resulting 
cost  of  a  false  detection  becomes  high  only  as  the  aircraft 
approaches  the  release  point.  This  cost  can  be  reduced  by 
disabling   filter   adaption   within    a   specified   time   to   go. 

Still  another  approach  attempts  to  adapt  the  filter 
bandwidth  by  adjusting  Q(k)  in  (3.11).  This  approach, 
investigated   in   [1]  and  [6]   calculates   Q  (k)     by 

Q(k)    =   a   Del(k)Del(k)       +    b    Del  (k-1)  Del  (k- 1)       (3.24) 

where    a   and   b    are  determined   by    data    analysis,    and 

Del(k)    =   X(k/k)     -   X(k/k-1)  (3.25) 

A  variation  of  this  technigue  that  looks  at  only  the  change 
in  the  highest  order  state  is  investigated  in  the  Kaiman 
filter   simulation. 

Most  of  the  discussion  thus  far  concerning  filter 
adaptation    has      been   directed   toward   Kaiman      filters.         Most 
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approaches  to  adapting  alpha-beta  filter  simply  open  the 
bandwith  by  switching  to  a  different  set  of  parameters  when 
a  maneuver  has  been  detected.  Reference  [4]  discusses  an 
adaptation  scheme  that  is  more  nearly  optimal  in  the  sense 
of  covariance  matching.  However,  for  the  sake  of  comparison 
the  simple  parameter  switching  technique  is  implemented  in 
the  alpha-beta  filter  simulation  subroutine,  since  that  is 
the  approach   used  in  the   AN/TPB-1D. 

F.       ESTIMATION    OF    PILOT    RESPONSE    TO    TARGET    BEARING    INPUTS 

As  discussed  in  the  previous  section,  aircraft  dynamics 
depart  from  the  filter  model  randomly  when  the  pilot 
responds  to  events  in  the  environment  and  deterministically 
when  he  responds  to  target  bearing  inputs  from  the  GDBS.  If 
his  response  to  these  inputs  were  known  with  some  degree  of 
certainty,  then  deteministic  forcing  might  be  included  in 
the  filter  model  in  a  manner  similar  10  that  found  in 
and  [  1  ]•  The  importance  of  identifying  parameters  which 
define  a  system  so  that  modern  control  strategies  can  be 
implemented      is        discussed      in   [7].  In      this        case      the 

parameters  would  be  those  that  describe  the  pilot/aircraft 
response   to   heading   inputs. 
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By  using  an  Autoregressive  Moving  Average  (ARMA) 
representation  for  the  pilot/aircraft  system,  and  Kalman 
filtering  to  process  the  heading-in  (actually  heading  error 
from  the  veiw  point  of  the  pilot),  bankangle  on:  data,  the 
coefficients  associated  with  the  ARMA  equation  could  be 
identified.  From  [7]  we  know  that  the  pilot/aircraft  system 
can    be  represented   by    the   ARMA   equation 


m 


n 


0(k)    =    £  a .   0t(k-j)    -     Lb,    0(k-:) 


3=0 


j    * 


j=i  3 


(3.26) 


where  the  present  bankangle  output,  $  (k)  ,  is  a  linear 
combination  of  pasx  outputs,  9  (k-i) ,  and  of  past  and 
present  heading  error  inputs,  9  (k) .  Estimating  the 
coefficients  of  this  ARMA  equation  can  be  formulated  as  an 
adaptive  Kalman  identifier,  where  the  heading  error  and  the 
bank  angle  are  simple  functions  of  The  velocity  and 
acceleration  state  estimates  generated  by  the  Kalman  state 
estimation  filter  previously  discussed. 

If  the  a.  and  b.  coefficients  of  the  ARMA  equation  are 
treated  as  states  of  the  pilot /aircraft  system,  then  the 
state  vector  becomes 


(3.27) 
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He     assume        that     these        coefficients     experience        random 
perturbations   so   that 


a^k+1)     =   a.(k)    ♦   w^k) 
bj(k+1)     =   bj(k)    ♦   w.(k) 


(3.28) 


Equation    (3.26)    then   becomes 

m 


n 


d(k)    =     £a,    a  (k-j)    -      £b.    0(k-j)    ♦   v(k)  (3.29) 


]=o 


3       S 


3=1  3 


where  w.(k)r  w  .  ( k)  ,  7  (k)  are  noise  processes  that  have  the 
same  statistics  described  for  the  Kalmar  filter  earlier. 
Combining    (3.28)     and    (3.29)     we    have 


— —                  ~™ 

a     (k+1) 

= 

a    (k) 

b    (k+1) 

b    (k) 

I —J 

♦       w(k) 


The  measurment  vector  is  defined, 


(3.30) 


[ 


H  (k)     =  0(k)       fl>(k-1) 


c 


•    •     0<k-m)     - 
-      0(k-1)   •    •  .    -    $( 


k-n)j 


(3.31) 


From   [7]   the    solution    is  then   formulated   as 


|     (k+1/k) 
b    (k+1/k) 


=      fl  -  S  (k)H(k)] 


■■ 

A 

a 

(k/k- 

■1) 

A 

b 

(k/k- 

'1) 

♦    G(k)     $(k)        (3.32) 


where 


G  (k)    =    P(k/k-1)  Hr(k)  [h  (k)?(k/k-1)  HT(k)     +    r]  (3.33) 
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P  (k/k)    =   P(k/k-1)     -   G(k)H(k)  P(k/k-1)    ♦   Q 


(3 .3U) 


Q   =    E 


w^k) 
Wj(k) 


[w.  (k)    w.  (k) 


(3.35) 


R    =   E    [v(k)     v(k)  ] 


(3.36) 


and 


P(k/k-1)     =   E 


a(k) 


b(k) 


|(k/k) 


S(k/k) 


a(k) 


b(k) 


|(k/k) 


b(k/k) 


(3.37) 


Initialization  of  the  sta tes(coef ficients)  and  the  error 
covariance  would  be  similar  to  that  discussed  in  the  next 
section. 

G.       FILTER    IMPLEMENTATIONS 

Three  separate  filter  subroutines  were  developed  to 
simulate  the  filtering  of  raw  radar  data  generated  by  the 
ground  radar  of  the  bombing  system  previously  described. 
All  three  filter  configurations  are  oriented  in  -he  three 
dimensional  Cartesian  coordinate  system  described  in  Chapter 
2  sines  the  aircraft  dynamics  are  assumed  to  be  more  nearly 
linear  and  well  behaved  than  in  the  polar  coordinate  system 
in    which      the    measurements      are   generated.  This   disparity 
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between  the  measurement  reference  frame  and  the  model 
dynamics  reference  frame  results  in  a  basic  nonlinearity 
when  transformations  are  required  frcm  one  frame  to  the 
other-  Probably  a      better  coordinate      frame    for      modeling 

aircraft  motion  would  be  one  that  translates  with  the 
aircraft  and  is  oriented  along  the  velocity  vector.  Such  a 
coordinate  system  was  found  to  be  very  awkward  and  difficult 
to  implement,  especially  considering  the  problem  of  the 
transformation   nonlinearity   just    mentioned. 

The  first  of  these  filters,  designated  ALF3TA,  is  a 
simple  sixth  order  alpha-beta  filter  with  the  parameter 
switching  adaptation  tecnique  described  in  the  previous 
section.  Adaption  is  initiated  when  a  heading  rate  cf  1 
degree  per  second  is  observed  for  period  of  5  seconds  or 
more.  The      second        and        third      subroutines        implement 

sixth  (KALMN1)         and      a inth (KALMN2)  order      Kalman      filters 

respectively.  Two  separate  adaptive  techniques,  which  were 
described  in  the  previous  section,  are  included  with  each  of 
these  filters.  The  first  of  these  adaptive  algorithms, 
designated  ADPTV1,  adjusts  the  Q  (k)  matrix  frcm  changes 
computed      in      the      highest        order      estimate.  The      second 

algorithm,       designated    ADPTV2,      simply   resets   the    covariance 
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of  error  matrix  P (k)  when  a  bias  is  detected  in  the 
innovations  sequence  for  more  than  one  second.  As  mentioned 
before,  the  difference  between  the  sixth  and  ninth  order 
filters  is  that  the  former  do  not  estimate  the  acceleration 
states  of  the  aircraft.  Tha  cost  of  this  additional 
information  provided  by  the  ninth  crder  filter  is  more 
computation  time   and  computer  memory. 

The        aircraft        model        is        formed        by        defining        a 
three-dimensional  Cartesian   state    vector 


X3=      [x    y   z] 


(3.38) 


where  x,  y,  and  z  are  each  one-dimensional  two  element  state 
vectors(positicn  and  velocity)  for  ALFBTA  and  KALMN1,  and 
three  element  state  vectors  (position,  velocity,  and 
acceleration)  for  KALMN2.  Tha  state  prediction  equations 
are  given  by  (3.1)  for  the  alpha-beta  filter  and  (3.10)  for 
both    Kaiman    filters.       For   the   Kalman    filter    configuration 


0(k)     = 


* 

0 

0 

0 

4> 

0 

0 

0 

♦. 

(3.39) 
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d(M    * 


4 

0 

o" 

0 

4 

0 

0 

0 

A 

(3.40) 


where      *  (k)    and      4(k)     are   defined    by    (3.41)       and    (3.42)      'for 
KALMN1    and    (3.43)    and     (3.44)    for    KALMN2. 


*(k)     ■ 


1      T 

0       1 


(3.41) 


4(k)      = 


2 

T72 


(3.42) 


*{k)     = 


1 

T 

T^2 

0 

1 

T 

0 

0 

1 

(3.43) 


4<k)      = 


2 

T72 


(3.44) 


0    and     ^     are   in    general      functions   of    k,         however    for 
this   simulation   they      are  not   since    a   constant      da~a    rate   is 
assumed   and   no   extended    predictions   are   required.         The    U  (k) 
matrix   would    be    utilized  to    include   deterministic    forcing   in 
the   model,         if   this   information      were   available      as    in    [ 1 ]- 
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However,  since  an  adaptive  Kalman  identifier  was  not 
implemented  to  estimate  the  pilot/aircraft  response,  no 
attempt  was  made  to  include  deterministic  forcing  in  the 
model. 

1 .  Kalman  Filter  Covariance  of  Measurement  Noise,  R  (k) 
Kalman  filter  theory  assumes  linear  relationships 
among  measurements  and  states  as  can  be  seen  from  (3.9). 
Since  aircraft  motion  is  modeled  in  a  Cartesian  reference 
frame  and  measurements  are  generated  in  a  polar  reference 
frame,  the  resulting  relationships  among  the  states  and 
measured  values  are  nonlinear,  as  can  be  seen  from  the 
transformation  eguations  shown   below. 

x   =    B   cos(E)     sin  (A)  (3  .45) 


y   =    B   cos(E)     cos  (A)  (3  .U6) 


z   =    R   sin(E)  (3  .47) 

Using     these        polar/Cartesian      transformations        to 
nonlinearly  combine  the  polar  observations, 

three-dimensional  Cartesian   measurements      are    generated    from 
(3.9)     to    form     (3.48)    below. 
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2x(k) 

R   cos(E)     sin  (A) 

vx(k) 

zy  (M 

s 

R  cos(E)     cos  (A) 

+ 

vk) 

zz  (k) 

R   sin(E) 

▼z(M 

(3. 48) 


where   the   observation    matrix  for   KALMN1    is 


H  (k)    = 


10    0    0    0    0 

0    0    10    0    0 

0    0    0    0    10 


(3.49) 


and  for  KALMN2  is 


H(k)  = 


100000000 
000100000 
000000100 


(3.50) 


In  order  to  compute  the  measurement  error  variance 
it  is  necessary  to  first  linearize  the  measurement  error. 
Differentiating  equation  (3.48)  with  respect  to  each  of  the 
measurement  variables  yields  (3.51)  ,  where  s  and  c  represent 
sin   and   cosine   respectively. 


J(x)  = 


sAcE   rcAcE 
cAcE   -rsAcE 
sE      0 


-rsAsE 
-rcAsE 
rcE 


(3.51) 


Thus  we   find  that  the  linearized   Cartesian  errors   can  be 
expressed  as 


=  J  (x) 


(3.52) 
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Therefore     assuming   no      cross      correlation   of  polar 

errors,          the        linearized     Cartesian        measurement  error 
ccvariance   matrix  is 

R    (k)    =    J(x)     R_(k)    J(x)T  (3.53) 


The  diagonal  terms   of    R    (k)     are 

R(1r1)     =    r2     (o2sE2sA2     ♦    a|cB2cA2)  ♦    a2cE2sA2                   (3.54) 

R(2,2)     *   r2     (a|sE2cA2     ♦    a|cE2sA2)  +    a2cE2cA2                  (3.55) 

R(3,3)    =    r2a2  cE2    +   a2  sE2  (3.56) 

The  off-diagonal   elements'are 

R(1,2)     =    r2a|  sE2sA2     +    ( a2  -    r2a2  )  cE2sAcA                        (3.57) 

R(1,3)    =     (0%    -    r2a2    )     sEcEsA  (3.58) 

R(2,3)     ♦    (o2    -    r2o|    )    sEcEcA  (3.59) 

and   due   to    the   symmetry  of    R  (k) 


R  (2,  1)     =    R  (1  ,2)  (3  .60) 

R  (3,  1)     =    R  (1  ,3)  (3.61) 

R  (3r2)     =    R  (2,3)  (3.62) 

It  should,  be  noted   t  hat  the  R(k)    matrix  is   not  constant 
since  it  depends  on  range,  azimith,  and  elevation. 
2 .   Filter  Initialization 

All  three  filters  were  initialized  with  reasonable 
state  values  fcr  position,  velocity,  and  acceleration  on  the 
the  first  pass  through  the  filter,   since  it  is  assumed  that 
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the  GDBS  has  maintained  a  good  track  for  sometime  prior  to 
the  final  leg  to  the  target.  Consequently  the  covarianca  of 
error  for  the  initial  state  prediction  vector  is  not  set  to 
an  arbitrarily  large  number  such  as  10  ,  as  is  often  done 
when  there  is  little  confidence  in  the  initial  state  values. 
Instead  10  is  used  since  it  is  more  consistent  with  the 
covariance    of     error  in     good   initial      state    values.  This 

simulated  pass  from  some  other  tracking  filter  to  the  filter 
of   interest   is   realistic  and  reduces   the  settling   Time. 

Program      constants   and      constant  array      calculations 

9 

for   the    Kalman    ft,    <P ,     A  $      and   3    matrices   are   set    up   on    this 

first      iteration.  The      process      noise      0      is      set      at      an 

arbitarily  small  number  to  ensure  that  the  gain  matrix  will 
not   converge   to    zero  and   accentuate    divergence    problems. 
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IV.       PRESENTATION    OF    RESULTS 

A.       QUALITATIVE    OBSERVATIONS 

Seven  different  filter  configurations  were  ultimately 
implemented  and  evaluated  with  the  GDBS  simulation.  These 
configurations  are        discusssd  below        and  assigned 

alpha-numeric    symbols    for  ease   of   reference. 

The      sixth      order    alpha-beta      filter,         ALFBTA  (AB)  was 

implemented  with  the  simple  parameter  switching  technique 
outlined  in  Chapter  3.  However,  the  heading  rate  maneuver 
detection  process  proved  to  be  too  insensitive  to  slow 
maneuvers  and  was  therefore  augmented  with  a  -rigger  that 
reset  filter  parameters  when  the  heading  error  exceeded  3 
degrees.  The      results        of      this      change      proved        to      be 

worthwhile,    as    will   ba    shown   later. 

The  next  three  filters  are  variations  of  the  sixth  order 
KALMN1  filter.  (K1 0)  is  XALMN1  without  adaptation,  (K11)  is 
KALMN1  with  the  process  noise  adaptation  scheme  fcr  modifing 
Q(k),  and  (K12)  is  KALHN1  adapted  by  resetting  P(k),  the 
error  covariance  matrix,  as  discussed  in  Chapter  3.  Filters 
(K20)  ,  (K21),  and  (K22)  are  variations  of  KALMN2  which 
correspond    to    the   KALMN1    variants    described    above. 
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Since  the  objective  was  to  evaluate  the  accuracy  and 
response  of  each  filter  in  the  final  phase  of  bomb  delivery, 
the  simulation  was  initialized  with  the  aircraft  within  90 
seconds  of  the  release  point,  at  a  speed  of  480  knots,  and 
on  a  heading  within  10  degrees  of  the  target  bearing.  For 
each  run,  after  allowing  5  seconds  for  the  filter  to  settle, 
Tacan  target  bearing  information  was  provided  to  the  pilot 
controller.  At  15  seconds  elapsed  time,  filter  adaptation 
was  enabled  and  random  maneuvers  were  begun  at  20  seconds. 
Five  separate  runs  were  evaluated  for  each  filter  where  the 
random  maneuvers  were  ceased  at  80,  60,  50,  40,  and  30 
seconds   prior   to   arrival  at   the   release   point. 

The  following  plots  were  generated  for  the  case  where 
maneuvers  were  stopped  with  a  time-to-go  of  50  seconds. 
Figures  4.1  through  4.21  show  true  and  estimated  (connected 
symbols)  position,  velocity,  and  acceleration  trajectories 
as  functions  of  time  for  all  filter  configurations.  Note 
that  the  target  position  is  designated  by  a  circle  on  the 
horizontal  trajectory  plot.  Also  representative  filter  gain 
schedules  are  included  to  show  the  effects  of  the  particular 
adaptation    process    being  utilized. 
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Figures  4. 1  and  4.2  indicate  the  magnitude  of  the 
maneuver  encountered  and  the  good  state  estimation  qualifies 
of  this  simple  sixth  order  alpha-beta  filter.  Notice  there 
is  no  significant  divergence  of  the  estimated  trajectory 
from  the  true  trajectory  throughout  the  run.  Figure  4.3 
shows  the  step  gain  adaptation  at  the  beginning  of  the  run 
in  response  to  the  controlled  turn  to  the  target  heading. 
This  gain  is  then  reduced  after  the  turn  is  completed  and 
before  the  first  random  maneuver  begins,  when  the  gain  is 
again    increased. 

The  trajectory  shown  in  Figure  4.4  contasts  sharply  with 
that  shown  in  4.1,  showing  significant  filter  divergence  for 
the  nonadaptive  sixth  order  Kalman  filter.  The  significant 
lag  in  velocity  state  estimation  shown  in  Figure  4.5  results 
from  the  convergent  gain  properties  characterized  in 
Figure   4.6. 

The  performance  of  K10  changes  dramatically  when  it  is 
made  adaptive  as  shown  in  Figures  4.7,  4.S,  and  4.9  for  K11 
and  Figures  4.10,  4.11,  and  4.12  for  K12.  Figure  4.9  shows 
continuous  gain  adjustment  in  response  to  perceived  changes 
in  the  process  noise.  Figure  4.12  shows  the  effect  of 
resetting   the    covariance  of  error    in    response    to   a    maneuver. 


47 


Figures  4.  13, and  4.14  show  the  improvement  in  state 
estimation  of  the  K20  nonadaptive  Kalman  filter  when  the 
order  is  increased  over  that  of  K10.  It  is  interesting  to 
note  that  we  see  significant  overshoot  in  the  velocity 
estimate   for   the    first    time.  Unlike   the   filters   discussed 

thus  far,  K20  provides  an  acceleration  estimate  which  can  be 
seen  in  Figure  4.15  and  accounts  for  the  sensitivity  of  the 
velocity   estimate. 

Adapting  K20  through  the  noise  process  technique  results 
in      K21         which      produces      the      position,  velocity,         and 

acceleration  estimates  shown  in  Figures  4.16,  4.17  and  4.18 
respectively.  Figures  4.19,  4.20,  and  4.21  provide  the  same 
information  for  K22,  which  represents  the  covariance  of 
error  adaptation  variant  of  K20.  These  last  two  adaptive 
filters  show  little,  if  any,  apparent  improvement  over  the 
nonadaptive  version.  This  observation  is  supported  in  the 
following   section. 

B.       QUANTITATIVE    RESULTS 

A  single  run,  for  each  filter  configuration  evaluated 
for  each  maneuver  termination  time,  is  not  sufficient  to 
properly  determine  filter  performance  over  the  range  of 
possible        maneuver      trajectories         and      measurement        noise 
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sequences.  Therefore,         30      simulation      trajectories      per 

filter,  per  maneuver  period,  were  conducted  with  different 
random  maneuver  and  measurement  noises  sequences  generated 
for  each  run.  The  bomb  release  signal  to  the  pilot  was 
assumed  to  occur  at  the  closest  point  of  approach  (CPA)  to 
the  target      release   point.  The    average     of   the      resulting 

CPA's  for  each  30  -est  runs  are  shown  in  Table  I.  CPA's 
greater  than  250  feet  are  classified  unsatisfactory  and 
labled    '0'    appropriately. 

At  a  glance  it  is  apparent  that  the  adaptive  sixth  order 
alpha-beta  filter  performs  very  well  in  such  a  dynamic 
environment,  except  when  maneuvers  are  continued  very  close 
to  the  target.  The  nonadaptive  sixth  order  Kalman  filter  is 
obviously  unsuited  by  itself,  but  when  made  adaptive, 
preforms  very  well,  particularly  for  the  process  noise 
adaptation  technique  when  maneuvers  are  terminated  late  in 
the  target  run.  The  ninth  order  Kalman  filter  performance, 
borh      adaptive      and      nonadaptive,  is      comparable      to      the 

alpha-beta  and  adaptive  sixth  order  Kalman  filters,  but  has 
problems  in  close  due  to  ins  longer  settling  time.  No-ice 
that  the  adaptive  variants  of  the  ninth  order  Kalman  have 
little  effect  on  that  filter's  performance,  as  we  surmised 
in    the   last   section. 
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TABLE  I 
Release  Point  Error  Tabla 


V 

\    r 

TTG  \ 
( sec)\ 

Alpha- 
Beta 

Kalman  (sixth  order) 

Kalman  (ninth  order) 

Adptv 

Adptv 

Adptv 

Adptv 

Adptv 

AB 

K10 

Kll 

■ 

K12 

K20 

■   K21 

K22 

80 

20' 

46  ■ 

38' 

39' 

30' 

30' 

30* 

60 

43  ■ 

U 

42  ' 

37* 

43  ' 

39' 

35  • 

50 

41' 

U 

75  • 

53' 

50' 

65* 

52  ■ 

40 

55  * 

u 

54  ' 

135' 

32* 

119' 

98' 

30 

230  ' 

u 

109' 

U 

247  ' 

0 

U 

Table  II  shows  the  relaxiva  cost,  in  computation  time 
and  memory,  to  implement  KALMN1  and  KALMN2  in  relation  to 
ALFBTA. 
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TABLE  II 
Relative  Computation  and  Memory  Costs 


Alpha-Beta  (sixth  ord) 

1 

Kalman  (sixth  ord) 

2.  9 

Kalman  (ninth  ord) 

3  .  4 

The   only   advantage   to  implementing   the   most   responsive 

variant  of   KALMN1  would  be  to   reduce  the  probability   of  a 

GDBS  generated   abort,   due  to   large  predicted   bomb  impact 

errors,  when  maneuvers  are  carried  very  close  to  the  release 

point-    Lastly,  if  the  adaptive  Kalman  identifier  proved  to 

be  useful  in  providing  deterministic   forcing  for  the  Kalman 

filter  model  and  resulted  in   improved  accuracy  and  response 

over  the  alternatives  presented,  the  cost  in  computation  and 

memory  resources   would  be   even  greater   than  we   have  seen 

here. 
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Fig.    4.3.       ALFBTA(AB)     Y-Gain    Schedule 
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Fig.    4.6.       KALMN1(K10)     Y-Gain   Schedule 
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Fig.    4.7.      KALMN1(K11)     Horizontal   Position   Trajectory 
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Fig.    4.10.      KALMN1(K12)     Horizontal    Position  Trajectory 
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Fig.    4.11.      KALMN1 (K12)     True   and    Estimated   Y-Velocity 
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Fig.    4.15.      KALMN2(K2  0)    True   and    Estimated    Y-Accelerati on 
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Fig.    4-16.      KALMN2(21)    Horizontal   Position  Trajectory 
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Fig.    4.17.      KALMN2(K21)     True    and    Estimated   Y-Velocity 
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Fig-    4.18.      KALMN2(K2  1)    True   and    Estimated    Y-Acceleration 
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Fig.    4.19.      KALMN2(K22)     Horizontal   Position  Trajectory 
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Fig.    4.21.      KALMN(K22)    True  and   Estimated   Y-Acceleration 
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♦3,1000) 

COMMON/TGTWND/TGTRRX,TGTRRY,TGTRRZ,TWCRR,TWVRR,EWDRR, 
+EWVRR 

COMMON/ PI LOT/ TB, SRTMVR , STPMVR, DT 

COMMON/NOISE/TACRMX( 1000), TACRMY( 1000), TACRMZ( 10C0) 

COMMON/FILTSR/TACREX(3,1000),TACREY(3,1000),TACREZ(3, 
♦1000) 

COMMON/ PR0CES/X2SIGM,Y2SIGM,Z2S J GM. EWVRRX, EWVRRY*GX( 
+  1000 ),G Y( 1000), GZ( 1000) ,RADRES( 1000)  ,XRES(1000) 
+  ,  YPES (1 000 ),ZRES( 1000) , Wll ( 1000  J , W22 ( 1000) , W33( 1000) , 
+P(1000,9) ,PINITL 

COMMON/ PAR AMT/G,G2,GK1,GK2 

DIMENSION    X(1000),Y( 1000) , TGTX( 1 ) , TGTY( 1 ) ,TRUACC( 1000) 
♦  , TIME (1000) ,X1(1000) , Yl ( 1000 ),ESTACC( 1000) 

C 

c 
c 

C  INITIALIZE    PARAMETERS; 

3NKANG=0.0 
DEADZN=0.0 
OLDHDE=0.0 
K  =  0 
K1=0 
K2=l 
TB=2.C 
PINITL=1000. 
SRTMV<?=20.C 
STPMVP=50.0 
IR0SFQ=8 
TTG=999. 
NTIMES=300 
SECCNO»0.0 
H0GERP=0.0 
G=32. 174045 
G2=16.C87CC7 
SK1=3.5 
GK2=3.5 

PI=3. 141592654 
OLDHDE=0.0 
DS=ED=    l.OCO 
RSIGMA=100. 
AS!GMA=.0Ol 
ESIGMA=.0Ol 
KFILTR=0 
KADAPT=0 
X2SIGM=1. 
Y2SIGM=1. 
Z2SIGM=1. 

C  READ    INITIAL    AIRCRAFT    STATES    IN    AC    REF.     FRAME. 

C 

C  WRITE(6j6) 

READ (5, I)  TACACX(1,1),TACACY( 1 , 1 ) ,TACACZ i 1 , 1 ) 
RE  AD (5,1)  TACACX(2,1),TACACY(2, 1),TACACZ( 2,1) 
READ (5, 1)    TACACX(3,1),TACACY(3,1),TACACZ(3,1) 
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OBTAIN    X    £    Y    ViIND    COMPONENTS    IN    RADAR    FRAME    &    ADD    TO 
C  CORRESPONDING    A/C    INITIAL    VELOCITY    STATES    TO    OBTAIN 

C  INITIAL    AIRCRAFT    STATES    IN    RADAR    REFERENCE    FRAME. 

C 
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c 


READ(5,2)  TGTRRX,TGTRRY,TGTRRZf TWDPR,TWVRR,EWDRR,EWVRR 
TWVRRX=TWVRR* SI N(TWDRR*P 1/180. 0)*1. 687805556 
TWVRRY=TWVRR*C0S(TWDRR*PI/180.0)*1.687905  556 
EW VRR X= EWVRR* S IN (EWDRR*PI/1 80. 0)*1. 6 87805556 
EW VRR Y= EW VRR* COS (EWDRR*P 1/180. 01*1.6  8780 5 556 
00    41    1=1,3 

IF    (  I    .NE.    2)    GC    TO    3 

TACRRX(2,1)=TACACX(2,1)    +    TWVRRX 
TACRRY(2,1)=TACACY(2,1)    +    TWVRRY 
TACRRZ<2,1)=TACACZ(2,1) 
GO    TO    41 
3  CONTINUE 

TACRRXU  ,1  )=TACACX(I  ,1) 
TACRRY(I,1)=TACACY(I ,1) 
TACRRZUflJ-TACACZUtlJ 
41         CONTINUE 

WRITE(6,4) 

WRITE(6,6> 

WRITE (6,8)  TACRRX<1,1) ,TACRRY(1, 1 ) , TACRRZ < 1 , 1 ) 

WRITE (6, 10)  TACRRX(2,1) ,TACRRY(2,1) ,TACRRZ(2,1) 

WRITE(6,12)  TWDRR,TWVRR 

WRIT=(6,14)  EwCRR,EWVRR 

WRITE(6,16)    RSIGMA,ASIGMA,ESIGMA 

WPITE(6,18)    X2SIGM,Y2SIGM,Z2SIGM 

WRITE(6,20)     PINITL 

WRITE(6,21)    KFILTR 

WRITF(6,22)    KAOAPT 

WRITE(6,24)     SRTMVR 

WRITE(6,26)    STPMVR 

WRITE(6,50) 

WRITE(6,53) 

WRITE(6,60) 

WRITE(6,63) 

WRITE(6,65) 
C 
C 
C  DETERMINE    RADAR    SAMPLE    I NTE RVAL ( CT) . 

DT=1.0/FL0AT< IRDSFQ) 
C 
C  PERFORM    SIMULATION    LOOP    N-TIM5S. 

DO    900    N    =    l.NTIMES 

ELPSTM    =    (N-l)    *    DT 

TIME(NI=ELPSTM 
C  NOW    CALL    PLPNCI    TO    TRANSFORM    RADAR    CARTESIAN    COORDS 

C  IN    POLAR,    ADO    WHITE    NOISE,    AND    RETPANSFQRM    INTO    RDR 

C  CARTESIAN    COORDS    IN    PREPARATION    FOR    FILTERING. 

C 

c 


c 
c 

c 


CALL    PLRNOI(RSIGMA,ASIGMA,ESIGMA,RANGE,AZMITH, 

+ELEVTN) 


IP  ( KFILTR  .NE.  0)  GO  TO  45 
CALL  ALFBTA(DT,HOGERR,TTG) 
GO  TC  59 

45  CONTINUE 

IF  (KFILTR  .NE.  1)  GO  TO  46 

CALL  K ALMN 1 ( KAD APT, DT,R SIGMA, AS  I GMA,ESIGMA, RANGE, 
+AZMITH,ELEVTN,TTG) 
GO  TC  59 

46  CONTINUE 

CALL    KALMN2 ( K ADAPT, DT ,RS I GMA,ASIGMA,E SIGMA, RANGE, 
+AZMITh,ELEVTN,TTG) 
59  CONTINUE 
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NOW  CALL  TACAN  SUBROUTINE  TO  OBTAIN  HEADING  TO  FLY 
C         TO  TGT. 

CALL  TACAN(TACHDG,DSRDGT,TACVEL,TTG,GTRNGtHDGERRJ 


C 


IF  (N  .GT.  800i  GC  TO  580 

IF  <M00(N-1,8)  .NE.  0)  GO  TO  550 

WRITE (6, 75)  ELPSTM,TACRRX(1,N),TACRRY(1,N), 
+TACRRZ(l,N),TACRRX<2fN),TACRRY(2,N) , TACRRZ< 2 t NI , 
+TACRRX(3,N),TACRRY(3,N),TACRRZ(3tN) 

WRIT  EC  6,  78)  TACREXU,N)  ,TACREY ( 1 ,N) , TACREZ< 
♦1»N),TACREX(2,N) ,TACREY(2tN) ,TACREZ<2,N) , 
+TACREX(3,N) ,T ACREY (3,N) ,TACREZ( 3 ,N) 

WRITE  (6,  78)  TTG,TACRMX(N),TACRMY<N)  , TACRMZ<N) 
♦,HOGERR,CSRDGT,TACHDG,TACVEL,GTRNG 

WRITE (6,78)  GX(N),GY(N)fGZ(N),XRES(N) ,YRES(N) 
♦,ZRES(N),W11(N),W22(N)  ,W33(N) 

WRITE (6, 78)  P(N,1)TP(N,2),P(N»3) ,P(N,4), 
+P(N,5),P(N,6) ,P<N,7) ,P(Nt8) ,P(N,9) 
550         CONTINUE 
580      CONTINUE 
C 
C         NO  CONTROL  INPUT  UNTIL  FILTER  SETTLES. 

IF  (ELPSTM  .LT.  5.)  GO  TO  800 
C 

C         DETERMINE  CONTROL  INPUT  TO  AC. 
C 

C         NO  CONTROL  INPUT  IF  TTG  IS  LESS  THAN  3  SECOND  OR  IF 
C         CLOSEST  POINT  OF  APPROACHi CPA )  HAS  BEEN  REACHED. 


C 
C 

c 


CALL  TTGCPA(N,K,TTG,GTRNG,IFLAG) 

IP  {((IFLAG)  .EQ.  1)  .AND.  (ABS(HCGSRR)  .GT.  DcADZN 
+))  GO  TO  600 

TACA0X(3,N)=0.0 
TACACY( 2,N)=C.O 
TACACZ<3,N)=0.0 
GO  TO  800 
C 

C  CHOOSE    MNVRPT    OR    CNTLPT    RESPONSE    BASED   ON 

C  MAGNITUDE    OF    HDGERR,AND    SRTMVR    ANC    STPMVR    CRITERIA. 

C 
600  CONTINUE 

IF     ( (ABS(HDGERR) )     .GT.    .523598776)    GO    TO    700 

IF    {(ELPSTM    .LT.    SRTMVR). OR.     (TTG    .LT.     STPMVR)) 
+  G0    TO    700 

CALL    MNVRPT(Kl) 
GO    TO    8C0 
C 

C  CALL    CNTLPT    SUBROUTINE    IF    MANEUVER    NOT    PERMITTED. 

C 

c 

700  CONTINUE 

CALL    CNTLPT(K2,HDGERR) 
C 

C  PROVIDE    MOCEL    SUBROUTINE    AC    STATES    AT    TIME    N. 

C 

800  CALL    XMODEL 

C 

C  MODEL    RETURNS    STATES    IN    AC    FRAME    FCR    TIME    N+l. 

C 

C  ADD    WIND    EFFECTS    OVER    DT    TO    OBTAIN    AC    STATES    IN 

C  RADAR    FRAME. 

CALL    WNDEFF(TWVRRX,TWVRRYfDT) 
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900 

1 
2 
4 
6 
8 

10 

12 
14 
16 

18 

20 
21 
22 
24 
26 
50 


53 
60 
63 
65 


75 

78 


CONTINU 

STOP 

FORMAT ( 

FORMAT! 

FORMAT! 

FORMAT! 

F3FMAT< 

♦SYSTEM! 
FORMAT! 

+  SYSTEM* 
FORMAT! 
FORMAT ( 
FG  RM  AT  ( 

♦E(RAO)) 
FORMAT ( 

+3< 10X»F 
FORMAT ( 
FO  RM  AT  ( 
FORMAT( 
FORMAT( 
FORMAT"! 

FORM  AT  ( 
+«TACRRZ 
♦•TACRRX 

FORMAT! 
♦,'TACRE 
*«TACREX 

FORMAT! 
+  »TACRMZ 
♦•TACHDG 

FORMAT! 
♦•X-RESI 
+t7Xt *W( 

FORMAT! 
♦•PKK44* 
+,8X,»PK 

FORMAT! 

FORMAT  I 

END 

SU8P0UT 

♦HOGERR) 

IMPL 


SIMULATION1 ) 


3F10.0) 
7F10.0) 

cay      ' T  fp^ 

/////2X,'«  INITIAL  COND   [ONS: 
//5X,« 

FT)  =  « 

5X 

FT 


5X 
5X 
5X 

5X 
1C 
5X 
5X 
5X 
5X 
5X 
•0 
lf 
3* 
•0 
X2 
31 
•0 


•A/ 

=  t 

«TR 
•ES 
•ME 
,3! 

•PR 
3)/ 
•ER 
«FI 
*FI 
•ST 

•ST 

,6X 
6X, 
6X, 
,18 
,6X 
6X, 
t22 


IN  RADAR  RE 
RADAR  REFE 


A/C  INITIAL  POSITION 

,3!10X,F1C.3J/) 

C  INITIAL  VELOCITY  IN 

,3!10X,F10.3)//) 

UE    WIND    =•  tlOXtF10.3f '/SIX 

T.    WIND   «»,10X1F10.3t,/iilX 

ASUREMENT    NCISE    VARIANCEStR 

10X,F10.3)/) 

CCESS    NOISE   VARIANCES!X,Y,Z 

I 

PCR  COVARIANCE  DIAGCNAL  =  •  , 

LTER  DESIGNATION  =,,4X,I1/) 

LTER  ADAPTATION  =',5X,I1//) 

ART  MANEUVER  = « , 10X  ,  F10 .3/) 

CP  MANEUVER  -'  ,10X , F10  ,3// 


,  F9.3 
,F9.3 
!FT)  , 

)    =S 


FERENCE 

PENCE 

/) 
//) 

A!RAD), 


SECOND 


6X 


STXt1 
7X,« 


•  t 
•0 

DU 
2,2 
•0« 
iSX 

K99 


t 6Xf •TACRRX1" , 
•TACRRX2* ,6X,,TACRRY2< , 
•TACRRY31  ,6X,»TACRRZ3«  ) 
X,«TACREX1» ,6X, • TACREY 1 
t,TACREY2i ,6X,»TACREZ2» 
•TACREY31 ,6X,«TACREZ3f ) 
X,«TTG» ,7X,,TACRMX» ,7X, 
FDGERR» ,7X,»DSRDGT« ,7X, 
TACVEL*  .SX^GTRNG'  ) 
X,«X-GAIN«  t7X,»Y-GAIN», 
, 'Y-RESIDU1 ,5X,*Z-RESIDU*,7 
7X, «W!3,3) •) 


16X, 
1CX,F10.3//) 


) 

TACRRY1" ?6X, 

TACRRZ2* ,6X, 


•  T6X,«TACREZ1«  ,6X 
,6X, 


TACRMY1 ,7X» 


///10F 
13X,9F 


19 
5X 
•t 

20X,»PKK11« 

•P 

// 


7X, 


Z-GAI 
X,«W! 


KK551 
/) 

13.4) 
13.4) 


8X 


t8X,,PKK22« 
•PKK66* ,8X, 


,8X,  «°K 
•PKK771 


K33' 
,8X. 


N«,5X, 
lfl)« 

3X, 

PKK88' 


INE    TACAN!TACHDG,DSRDGT,TACVEL,TTGtGTR 
ICIT    REAL!A-H,Q-Z),INTEGER(I-N) 


NGf 


DOUBLE    ORECISICN    DSEED 

COMMCN/DOUBLE/DSEED 

COMMCN/TMRRST/N,TACRRX!3,1000),TACRRY(3,1000) , 
+TACRRZ (3,1000) 

COMMCN/TGTwND/TGTRRXfTGTRRYtTGTRRZ,TWDRR,TWVRRT 
+EWDRR,EWVRP 

COMMON/ PILGT/T 3, SRTMVR, STPMVR , DT 

C0MMCN/FILTER/TACREX!3,1000) , TACREY! 3 , 1000 ) , 
+TACREZ! 3,1000) 

C0MMCN/PAR*MT/G,G2,GK1,GK2 

"1*3. 141592654 

EW0=EWDRR*PI/180.0 

FWV=EWVRR*1.6  87  80  556 

TACVEL=SQRT(TACREX!2,N)*TACREX(2,N)+TACREY!2,N)* 
+TACREY(2,N) ) 

TACH0G=TRUhDG!TACREX!2,N) ,TACREY!2,N) ) 

TG7DIR=TRUFCG< ( TGTRRX-TACREX! 1 ,N) ),!TGTRRY- 
+TACREY!1,N) ) ) 

OSRDGT=TGTCIP 

HOGERR=CSRDGT-TAChDG 

GTRNG=SQRTUTACREX!  1 ,  NJ-TGTRRX)  **2  +  <  TACREY  (  1,N  )  - 
+T^T$RY)**2) 

TTG=GTRNG/TACVEL 
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RETURN 
END 
C 
C 
C 

SUBROUTINE    TTGCPA(N, K,TTG, RNGt I  FLAG J 

IF    (<TTG    .GT.    3.0)    .OR.    (K    .EO.    0))    GO    TO    600 
GC    TO   700 
600  CONTINUE 

IP    (N    .LT.    100)       0L0RNG=RNG 
DIFFNOOLDRNG-RNG 
IF    (TTG    .GT.    10.)    GO    TO    800 
IF    (DIFFNC)    700t800t800 
700  IFLAG=-1 

K=l 

GC    TO    9C0 
800  I*LAG=1 

900  CONTINUE 

OLDRNG=RNG 
RETURN 
END 
C 
C 
C 

SUBROUTINE    MNVRPT(Kl) 

DOUBLE    PRECISION    DSEEC 
COMMON/DOUBLE/CSEED 

CQMM0N/TMACST/N,TACACX<3, 1000 ) , TACACYi 3, 1000) , 
♦TACACZ<3,1000) 

CnMMCN/PILOT/TB,SRTMVR,STPMVRtOT 
C0MM0N/PARAMT/G,G2,GK1,GK2 
IF  ( <N-K1)  .EQ.  1)  GO  TO  100 
90  CMDACC^C.O 

N1=0 

PA=GGUBFS(DScED) 
AMXACC=14.0*<PA-.5) 
ACCDUR=20. 0*EXP< .25* ABS ( AMXACC ) ) 
103  CONTINUE 

ACCDURaACCCUR-OT 
IF    (ACCDUR    .L5.    0.0)     GC    TO    90 

CMOACC=AUXACC*{  1. -EXPC-DT/TB )  )+C.MDACC*EXP  < -DT/TB ) 
AACKDG=TRUHDG<TACACX(2,N),TACACY< 2tN) ) 
TACACX< 3,N)=CMDACC*CCS<AACH0G)*G 
TACACY(3,N)=-CMDACC*SIN(AACHDG)*G 
TACAC2(3,10CO)=C.O 
K1=N 
RETURN 
END 
C 

c 

c 

SUBROUTINE    CNTLPTC K2 ,HDGERR ) 
C  GENERATE    CCNTROLED    PILCT    BANKANGLE. 

C  PHD1     IS    COMPONENT    BASED    ON    ANGLE    ERROR    HDE. 

C  PHD2    IS    COMPONENT    BASED    ON    ANGLE    ERROR    PATE    hDEDOT, 

C  =>HD    IS    DESIRED    PILOT    GENERATED    BANK    ANGLE. 

DOUBLE    PRECISION    DSEED 

COMMON/0CU3LE/DSESD 

C0MMCN/7MACST/N,TACACX<3, 1000), TACACY( 3,1000) , 
+TACACZ(3,1000) 

C0MMCN/PARAMT/G,G2,GK1,GK2 


COMMCN/PILCT/TB,SRTMVR,STPMVR,DT 
15=    UN-K2)     .EQ.    1)    GO    TO    200 


OLDHDS=G.O 
3NKANG=G.O 
N2=0 
200  CONTINUE 
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N2=N2+1 

HDEDCT=(HDGERR-OLDHCE)/DT 
PH01=GK1*HDGERR 
PHD2=GK2*H0EDOT 
C*DANG=PHD1+PHD2 

IF    <A8S(HDGEAR)    .GT.    .1745)    GG    TO    202 
IF    (CMDANG    .GE.    .5236)       CMDANG=.5236 
IF    (CMDANG    .L£.    -.5236)       CMDANG=-.5236 
GO    TO    204 
202  CONTINUE 

IF    (CMDANG    .GE.    1.047)       CMDANG=1.047 
IF    (CMDANG    .LE.    -1.047)       CM0ANG=-1. 047 
204  CONTINUE 

BNKANG=CMDANG*(l.-cXP(-DT/TBJ)+BNKANG*EXP(-OT/TB) 
AACVEL=SQRT(TACACX(2,N)*TACACX(2,Ni+TACACY(2,N)* 

♦  TACACY(2,N)  ) 

AACHCG=TRUHDG(TACACX(2,N) ,TACACY(2,N) ) 

HOGR  AT=TAN ( BNKANG )*G/AAC VEL 

TACACX(3,N)=AACVEL*HDGRAT*C0S(AACHCG) 

TACACY(3,N)=-AACVEL*HDGRAT*SIN(AAChDG) 

TACACZ(3,N)=0.0 

GLDHDE=H0GERR 

K2  =  N 

RETURN 
END 
C 
C 
C 

SUBROUTINE    XMODEL 

DOUBLE    PRECISION    DSEED 

C0MMCN/DCU6LE/CSEED 

COMMCN/TMACST/N,TACACX(3,1000),TACACY(3,1000), 
+TACACZ(3,1000 ) 

COMMON/ P I LCT/T8,SRTMVR,STPMVR,DT 

C?MMQN/PR0CES/X2SIGM,Y2SIGM,Z2SIGM 

C0MMCN/PARAMT/G,G2,GK1,GK2 

OIMENSION  PHI(6,6),DEL(6,3) ,X(6,1),U(3,1) ,F<6,1), 

♦  XNH6,1),X1(6, 1) 

T2=(CT*DT) /2 
IF  (N  .NE.  1)  GO  TO  160 
C  COMPUTE  STATE  TRANSITION  MATRIX(PHI). 

C  INITIALIZE  MATRIX  3  0.0  ,  1.0  ON  DIAGONAL. 

DO  130  1=1,6 

DO  120  J  =  l,6 

PHK  I ,J)=0.0 

IF  (  I  .EQ.  J)   PHKI  ,J)  =  1.0 
120  CONTINUE 

130         CONTINUE 

PH!(1,2)=DT 
PHI(3,4)=DT 
PHI(5,6)=DT 
C  COMPUTE  DEL  MATRIX(DEL) 

C  INITIALIZE  a  0.0. 

DO  150  1=1,6 

DO  140  J=l,3 

DEL( I, J)=0.0 
140  CONTINUE 

150         CONTINUE 

DEL(1,1)=T2 
DEL(3,2)=T2 
DEL(5,3)=T2 
DEL(2,1)=DT 
0EL(4,2)=DT 
DEL(6,3)=CT 
C 
C 
C  SET  UP  »Xf  MATRIX. 
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160  X<1,1)=TACACX<1,N) 

X(2,1)=TACACX(2,NJ 

X(3,1)=TACACY< 1,N) 

X(4,1)=TACACY(2,N) 

X<5,1)=TACACZ( 1,N) 

X(6,1)=TACACZ(2,N) 
C 
C  CALL    PRCDUCT    SUBROUTINE    TO    MULTIPLY    PHI    £   X 

CALL    PRCCCT<PHI,X,6,1,6,X1J 
C 

C  SET    UP    'U*    MATRIX. 

C 

iJ(ltl)  =  TACACX(3,NJ 

U(2,1)  =  TACACY(3,NJ 

U(3,1)=TACACZ(3,N) 

C  CALL    PROCUCT    SUBROUTINE    TO    MULTIPLY    DEL    &   U 

CALL    PRCDCT(DEL,U,3,1,6,F) 
C 

C  CALL    ADD    SUBROUTINE   TO    ADO    X    6    F    TO    OBTAIN 

C  STATES    AT    TIME    (N+l). 

C 


C 
C 


END 


CALL    AD0(X1,F,1,6,XN1) 


IP=N+1 

TACACX(1,IP)=XN1<1,1) 
TACACX<2,IP)=XN1<2,1) 
TACACX(3,IP)=TACACX(3,N) 
TACACY( 1,IP)=XN1<3,1) 
TACACY(2f  IP)=XNU4,1) 
TACACY(3,IP)=TACACY(3,N) 
TACACZC  1,IP)=XN1(5,1) 
TACACZ<  2,IP)=XN1(6,1) 
TACACZ(3,IP)=TACACZ(3,N) 

RETURN 


C 

c 

c 

C  THIS  SUBROUTINE  COMPUTES  THE  MATRIX  PRODUCT  A*8  AND 
C  STORES  RESULT  IN  C. 

SUBROUTINE  PRGCCT( A, B, N,M, L ,C ) 
DIMENSION  A(L,N)  ,B<N,M)  ,C(L,M) 
C 

C         INITIALIZE  »C«  MATRIX3  0.0. 
^0  180  1=1, L 

DC  170  J=1,M 
C( I, J»=0.0 
170         CONTINUE 
180      CONTINUE 
C 

C         MULTIPLY. 
C 

DO  210  I=1,L 

00  200  J=1,N 

DC  190  K=1,M 

C(  I,K)=C(I,K)+AU  ,  J)*8(  J»K) 
190  CONTINUE 

200         CONTINUE 
210      CONTINUE 
RETURN 
END 
C 
C 

c 
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SUBROUTINE  ADO (A, B,L,M ,C ) 

DIMENSION  A(M,L),8<M,L) ,C(M,L) 

c 

INITIALIZE  «C«  3  0.0. 

DO  215  1=1, M 

DO  213  J  =  1,L 

C<I, J)=0.0 

213 

CONTINUE 

215 
C 
C 
C 

CONTINUE 

NOW  ADD  'A'  £  »B» . 

DO  230  I=1tM 

DO  220  J=1,L        t   , 
C(I, J)=A(I, J)+B(I,J) 

220 

CONTINUE 

230 

CONTINUE 

RETURN 

END 

C 

C 

C 

c 
c 
c 


SUBROUTINE    WNCEFF( TWVRRX,TWVRR Y, DT) 

COMMGN/TMACST/NT,TACACX(3,1000J,TACACY(3,10QO)  , 
+TACACZ<3,1000) 

COMM0N/TyRRST/N,TACRRX(3,10OC),TACRRY(3,10GC) , 
+TACRRZ(3,1000) 

IP=N+I 

TACRRX(1,IP)=TACACX(1,IP)+DT*TWVRRX*N 

TACRRX(2,IP)=TACACX(2,IP)+TWVRRX 

TACRRX(3,IP)=TACACX(3,IP) 

TACRPY(1,I P)=TACACY( 1,IP)+DT*TWVRRY*N 

TACRPY(2,IP)=TACACY(2,IP)+TWVRRY 

TACRRY(3,IP)=TACACY(3,IP) 

TACRRZ(1,IP)=TACACZ(1,IP) 

TACRRZ<2,IP)=TACACZ(2,IP) 

TACRRZ(3,IP )=TACACZ(3,IP) 

RETURN 
END 


FUNCTION    TRUHCG(XVEL,YVEU 
oi=3.141592654 
IF    (XVEL)     31,41,51 
31  IF     (YVEL)     71,111,71 

41  I<=     (YVEL)     101,91,121 

51  IF     (YVEL)     61,91,61 

61  TRUHDG=PI/2.0    -    ATAN ( YVEL/XVEL ) 

GO    TO    131 
71  TRUHDG*3.0*P 1/2.0    -   ATANt YVEL/XVEL ) 

GO    TO    131 
91  TRUHDG=PI/2.0 

GO    TO    131 
101  TPUHDG=PI 

GO    TO    151 
111  TRUHDG=3.0*PI/2.Q 

GC    TG    131 
121  TRUHDG=0.0 

131  RETURN 

END 
C 
C 
r 

C  THIS    SUBROUTINE    TRANSFORMS    CARTESIAN    COORDS    INTO 

C  POLAR     IN    THE    RCR    REF.    FRAME    THEN    AOOS    WHITE    NOISE    OF 

C  0    ME*N    AMD    VARIANCE,    RS  IGMA, AS  I GMA,     AND    ESIGMA    TO    THE 

C  RANGE,     AZIMITH,    AND    ELEVATION    RESPECTIVELY.    THESE 
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C  CONSTITUTE    THE    MEASURED    POSIT   OF    THE    A/C.    THESE    NOISY 

C  POLAR    MEASUREMENTS    ARE    THEN    TRANSFORMED    BACK    INTO    THE 

C  RADAR    CARTESIAN    SYSTEM    IN    PREPARATION    FOR    FILTERING, 

C 
C 

SUBROUTINE    PLRNOURS  IGMA,AS  IGMA,  ES  IGMA,RNGPNS  ,  AZI  PNS  , 
+ELV»NS) 

DOUBLE    PRECISION    CSEED 

COMMON/DOUBLE/ DSE ED 

COMMON/ TMRRST/N , T ACRRXC 3, 1000 ),TACRRY( 3,1000), 
♦TACRRZC  3,1000) 

COMMON/NOI  SE/TACRMXUOOO)  , TACRMYC 1000)  ,TACRMZ<  1000) 

COMMON/ PARAMT/G,G2,GK1,GK2 
C 
C 
C 

SLNTRG=SCRT(TACRRX(1,N)**2+TACRRY<1,N)**2+ 
+TACRRZ(1,N)**2) 

AZMITH=TRUFCG(TACRRX(1,N),TACRRY( 1,N) ) 

GNDRNG=SQRTITACRRXU,N)**2+TACRRY(1,N)**2) 

5LVATN=ATAN(TACRRZ< 1 ,N)/GNDRNG) 

R=GGNQF(CSEED) 

RNGPNS=SLNTRG+R*RSIGMA 

A=GGNCF(0SEED) 

AZIPNS=AZMITH+A*ASIGMA 

5=GGNQF(DSEED) 

ELVPNS=ELVATN+E*ESIGMA 

TACRMX(N)=RNGPNS*COS(ELVPNS)*SIN(AZIPNS) 

TACRMY(N)=RNGPNS*COS(cLVPNS)*COS<AZIPNS) 

TACRMZ(N)=RNGPNS*SIN(ELVPNS) 

RETURN 
END 
C 
C 

c 
c 

SUBROUTINE    ALPBTA( CT , HDGERR , TTG ) 

COMMCN/TMRR ST/N, T ACRR X( 3, 1000 ),TACRRY (3,1000), 
+TACRRZ(3,1000) 

COMMCN/NOISE/TACP.MXUOOOJ  ,TACRMY(  1000)  ,TACRMZ<  1000) 

COMMON/ FILTER/TACREX (3,1 000) ,TACREY(3, 1000), 
+TACRHZ(3,1000) 

C0MMCN/PR0CES/X2SIGM,Y2SIGMfZ2SIGM,EwVRRX,£WVRRY, 
+  GX  ( 1000  ),GY(  1000)  ,GZ(1000-)  ,RA0RES(  10  00)  ,  XRES  ( 1000  )  , 
+YRES(1000),ZRES< 1000 ), W1M 100 ) ,W22( 1000) , W33 { 1C00) , 
♦  P(  1000,9)  ,PINITL 


DT2=07*0T 

I*=    (N    ,N£.     1)    GG    TO    10 


C 

C  INITIALIZE    FILTER. 

PPXKK=TACRYX(N) 
FPYKK=TACRMY(N) 
FPZKK=T4CRMZtN) 
FVXKK=TACRRX(2,N)+EWVRRX 
FVYKK=TACRRY(2,N)+EWVRRY 
PVZKK=TACRRZ(2,N) 
F°XKK1=FPXKK+0T*FVXKK 
F°YKK1=FPYKK.+  DT*FVYKK 
FPZKK1=FPZKK*DT*FVZKK 
STRGER=0.0 
STRGRT=0.0 
MVPFLG=0 
MVRCNT=0.0 

C 

C 
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GO  TO  999 
10    CONTINUE 
C 
C 

C      SELECT  PARAMETERS  FOR  FILTERING  BASEC  ON 
C      MANEUVERING  AND  NOISE  CRITERIA, 
C 

C      HIGH  OR  LOW  NOISE  ENVIRONMENT? 
C 

RUFRNG=SQRT(TACRMX(N)**2+TACRMY(N)**2+TACRMZ<N)**2) 
IF    (Q.UFRNG    .GT.    1500. i    GO    TO    20 
NOSFLG=0 
GO    TO    30 
20         CONTINUE 
N0SFLG=1 
30         CONTINUE 
C 

C      MANEUVERING? 

C      IF  TTG  .LT.  30  SEC  ASSUME  NO  MANEUVERING  AND  GC  ON. 
C 

IF  (TTG  .LT.  3C.I  GO  TC  40 

CALL    STRG( CT.NGSFLG, FDGERR , STRGER , STRGRT) 
C4LL    TMNVR(STRGRT.MVRCNT) 

IF     ( <DT*FLOAT(MVRCNT) )    .GT.    5.01     MVRFLG=0 
IF    < (CT*FLOAT<MVRCNT) )    .LT.    -5.0)    MVRFLG=0 
IF    (ABS(HDGERR)     .GT.     .052359878)     MVRFLG=1 
GO    TC    50 
40         CONTINUE 
MVRFLG=0 
50         CONTINUE 
C 
C  FIND    ALFA/BETA    PARAMETERS    . 

CALL    CRSTRK(MGSFLGtMVRFLG,TTG,CALFA,C3ETA) 
GX(N)=CALFA 
C 

I 

CY  (N)=CALFA 
GZ(N)=CALFA 

C 

C  FILTER    X-CGORC.    DATA. 

C  CURRENT    STATES. 

C 

C 

XRES0U=TACRMX(N)-FPXKK1 

FPXKK=FPXKK1+CALFA*XRESDU 

FVXKK=FVXKK+CBETA*XRSSDU/DT 

C  PREDICTEC    STATES. 

C 

FPXKK1=FPXKK+FVXKK*DT 
C 

C      FILTER  Y-COORD.  DATA. 
C      CURRENT  STATES. 
C 
C 

YRSSOU»TACRMY (N)-FPYKKl 

F°YKK*FPYKK1+CALFA*YR£SDU 

FVYKK=FVYKK+C8ETA*YRES0U/DT 

C  PREDICTED    STATES. 

C 

FPYKK1=FPYKK+FVYKK*DT 
C 

C      FILTER  Z-CCORC.  DATA. 
C      CURRENT  STATES. 
C 
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ZRESDU-TACRMZINI-FPZKKI 

FPZKK=FPZKK1+CALFA*ZRES0U 

FVZKK-FVZKK+CBETA*ZRESOU/OT 

C 

C  PREDICTED    STATES. 


C 


c 
c 
c 
c 


FPZKK1=FPZKK+FVZKK*DT 

XRES(N)=XRESDU 
YRES<N)=YPESDU 
ZRES(N)=ZRESDU 

RACRES(N)=SQRT(XRESDU**2+YRESDU**2+ZRESDU**2) 
999       CONTINUE 

TACREX( 1,N)=FPXKK 

TACREY(1,N)=FPYKK 

TACREZ< 1,N)=FPZKK 

TACREX( 2,N)=FVXKK 

TACREY(2,N)=FVYKK 

TACREZ(2,N)=FVZKK 

RETURN 

END 


SUBROUTINE    STRG( OT ,NOS FLG,HDGERR , STRGER, STRGRT ) 
IF    (NOSFLG    .EQ.    1 )    GO    TO    15 
ALFA=.0625 
BETA=.0C78125 
GO    TO    20 
15  CONTINUE 

ALFA=. 02125 
BETA=.0C1953125 
20  CONTINUE 

STRGER=STRGER+ALFA*(HCGSRR-STRGER) 
STRGRT=STRGRT+BETA*(HCGERR-STRGER) 
STRGER=STRGER+STRGRT*DT 
RETURN 
ENO 


C 

c 

c 
c 


SUBROUTINE    TMNVR ( STPGRT , MVRCNT ) 

Ic     (ABS(STRGRT)     .GT.    .00872665)    GC    TO    10 
IF    (MVRCNT. LT.    0)     MVRCNT=0 
MVPCNT=MVRCNT+1 
GO    TO    30 
10  IF     (ABS(STRGRT)     .LT.     .0174533)    GO    TO    20 

IF     (MVRCNT    .GT.    0)    MVRCNT=C 
MVRCNT=^VRCNT-1 
GO    TC    30 
20  MNRCNT=0 

30  CHNTINUE 

RETURN 
END 


SUBROUTINE    CRSTRKC NOSFLG, MVRFLG, TTGt CALF A ,C86TA) 
IF     (NCSFLG    .EG.    0)    GO    TO    50 

IF    (MVRFLG     .EQ.    0)    GO    TO    10 
LOW    PRECISION    (MANEUVERING) 
CALFA=.1875 
C8ETA=. 0073125 
GO    TC    30 
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10  CONTINUE 

C  HIGH    PRECISION    < NGN-MANUEVERING) 

IF  (TTG  .LT.  30.)G0  TO  20 
C  MANUEVERING  STOPPED  WITH  .GT.  30  SEC  TQ  GO. 

CALFA=. 0390625 
CBETA=.0C02441406 
GO    TO    30 
20  CONTINUE 

C  MANUEVERING    STOPPED    WITH    .LT.    30.    SEC    TO    GO. 

CALFA=. 078125 
CBETA=.0C09765625 
30  GO  TO  99 

C         LOW  NOISE. 
50       CONTINUE 

IF    (MVRFLG    .EQ.    0)    GO    TO    60 
C  LOW    PRECISION    (MANEUVERING) 

CALFA=.375 
C8ETA=. 03125 
GC    TC    30 
60  CONTINUE 

C  HIGH    PRECISION    (NON-MAN EUVER  ING J 

IF    <TTG    .LT.    3C.)    GO    TO    70 
C  MNVRNG    STOPPED    WITH    30    OR    MORE    SEC    TTG. 

CALFA=. 078125 
CBETA=. 0009765625 
GC    TO    80 
70  CONTINUE 

C  MNVRNG    STOPPED    WITH    LESS    THAN    30    SEC    TTG. 

CALFA=. 15625 
CeETA=.0039C625 
80  CONTINUE 

99  CONTINUE 

RCTURN 


C 
C 


c 
c 
c 


c 

c 
c 


c 

c 


END 

SUBSnuTTN*    KALMN1< KAD,DT ,RS IGMA, ASI GMA, ESIGMA , RANGE , 
♦AZMITHt 6LEVTN,7TG) 


DOUBLE    PRECISION    CSEED 

COMMON/ DOUBLE /CS EEC 

COMMON / TMRRST /N, T ACR RX (3, 1000 ),TACRRY< 3, 1000) , 
+  TACC?'5Z(3110002 

COMMON/NOISE/TACRMX( 1000), TACRMY (1000) ,TACRMZ< 1000) 

C:MM0N/FILTER/TACREX(3, 1000),TACRSY(3,1QQ0) , 
♦TACREZ(3,10CO) 

COM«ON/PROCES/X2SIGM,Y2SIGM,Z2SIGM,SWVRRX,EWVRRY, 
+GX(1000) , GY ( 1  COO ), GZ ( 1 000 ),RA0R£S( 1000) , XRES(  1000) , 
+  YPE5(  10CC)  ,ZR5S<  1QC0)  ,U IK  1000)  ,  W22  (  1000) 
+,W33(10CC),°<  1000,9)  ,PINITL 

DIMENSION    Q(6,6)  , PKK (6 ,6 ) , R (6, 6j  ,W(6,o),G<6,6), 
♦PHI (6,6) ,DEL(6,6) ,0I(6,6),NULL(6,6) ,H(6,6),XKK<6), 
+  XKKK6)  , £K1(6  )  ,  EXTRA  Ho,  6)  ,  EXTRA2(  o  ,  o  )  ,  PKK1  (  6  ,  6  )  , 
+WKAREA( 13) , EXTRA 3 (3, 3) ,EXTRA4<3,3) ,Z(6) 


IF    (N    .NS.    1     )    GO    TO    25 

IDGT=2 

CT2=0T*DT/2.0 


DO    20    1=1,6 
Z(I  )=?.0 
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c 

c 

c" 


DP  10  J=l,6 

Q(I,J)=0.0 

PKK<I ,J)=0.0 

IF  (I  .EQ.  J)  PKK(I,J)=PINITL 

R(I,J)-0.0 

W(I,J)  =  C0 

G(I,J)=0.0 

PHI(I,J)=0.0 

DEL( I,J)=0.0 

OKI,  J)=0.0 

IF  (  I  .EQ.    Ji  DKIt  Ji  =  1.0 

NULLU,  J)=0.0 

H( I,J)=G.0 
10      CONTINUE 
20    CONTINUE 

U1=X2SIGV*X2SIGM 

W2=Y2SIGM*Y2SIGM 

W3=Z2SIGM*Z2SIGM 

WX=0.0 

WY=WX 

V«Z=WY 

IXFLG=0 

IYFLG=0 

IZFLG=0 

DEU 1,1)=0T2 

DFL(2,1)=0T 

DEU3,2)=0T2 

DEL(4,2)=DT 

0EL(5,3)=DT2 

DEL(6,3)=DT 

H(  1,1)=1.0 

H( 2,3)=1.0 

H<3,5)=1.0 

PHI  (1,1)  =1.0 

PHI(1,2)=0T 

PHI (2, 2) =1.0 

PHI(3,3)=1.0 

PHI(3,4)=DT 

PHI(^,4) =1.0 

PHI(5,5)=1.3 

PHI(5,6)=DT 

PHI(6,6)=1.0 

PVAR=RSIGMA*RSIGMA 

AVAR=ASIGMA*ASIGMA 

EVAR=ESIGMA*SSIGMA 

TACREX(1,1)=TACRMX(1) 

XKK<1)=TACREX<1,1) 

TACRSY(1,1)=TACRMY<1) 

XKK(3)=TACREY<1,1) 

TACREZ(1,1)»TACRMZ<1 ) 

XKK(5 )=TACPEZ( 1,1) 

TACRFX(2,1)=TACRRX(2,N)+EWVRRX 

XKK(2 )=TACREX<2,1) 

TACP=Y(2,1)=TACRRY(2,N)+EWVRRY 

XKK(4)=TACRSY(2,1J 

TACRSZ( 2,1)=TACRRZ(2,N) 

XKK(6)=TACREZ(2,1) 

IF    (N    .EQ.    1)    GO    TO    999 
2f  CONTINUE 


Z(  1)=TACPMX(NJ 
Z(2)=TACRMY(N ) 
Z(  3)=TACPMZ(N) 
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c 

c 


c 
c 
c 


R2=RANGE*RANGE 

CA=COS(AZMITH) 

SA=SIM(AZMITHJ 

CE=COS(ELEVTNi 

SE=SIN<ELEVTN) 

CA2=CA*CA 

SA2=SA*SA 

CE2=CE*CE 

SE2=SE*SE 

P( li 1  J  =  R2*(  EVAR*SE2*SA2+AVAR*CE2*CA2)+RVAR*CE2*SA2 

R(2,2 )=P2*(EVAR*SE2*CA2+AVAR*CE2*SA2I+RVAR*CE2*CA2 

R(  3,3)=R2*EVAP*CE2+RVAR*SE2 

R(  1,2>  =  '5  2*EVAR*SE2*SA*CA  +  (RVAR-R2*AVARJ*(CE2*SA*CA) 

R(2,1)=R(1,2) 

R(  l,3)=(RVAR-R2*EVAR)*Sc*CE*SA 

R(3,1)=R(1?3) 

R(2»3)=(RVAR-R2*EVAR)*SE*CE*CA 

R( 3,2)=R(2,3) 


FK1(1)=0.0 
FK1(2)=0.0 
FK1(3)  =  C0 
FK1(4)=0.0 

FK1<5)  =  0.0 
FK  1(6)  =  0.0 


IF    (KAD    .NE.    0)    GO    TO    29 

w(  i,  i)=m 


W(2,2)=W2 
W( 3,3)=W3 

GO    TC    34 
29         CONTINUE 

IF     (KAD    .NE.     1)    GO    TO    31 

CALL    A0PTVHN,6,120tWl«W2tW3*TACREX,TACREYfTACREZ, 

+  W) 

GO  TC  34 
31    CONTINUE 

IP  (TTG  .LT.  20.)  GO  TO  33 
XRS=XRES(N-1) 
YRS=YRES<N-1) 
Zt?S=ZRES(N-l) 
CALL  ADPTV2(6,XRS,IXFLG,YRS,IYFLG,ZRS, IZFLG,PKK) 

33  CONTINUE 
W(l, 1)=W1 
W(2,2)=W2 
W< 3, 2)  =  W3 

34  CONTINUE 
Wll(N)=W(l,l) 
W22(N)=W(2,2) 
H33(N)=W(3,3) 

C 

C  GENERATE  Q-MATRIX. 

CALL  QUADPS(DSUW,NULL,6,6,Q) 
C 
C 

C  X(K/K-1)=PHI(K,K-1J*X(K-1/K-1)+F(K-1J 
CO  210  1*1,6 

XKK1(I)=FK1(I) 
DO  200  J=l,6 

XKK1(I)=XKK1< I )+PHI(I , JJ*XKK( J) 
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200      CONTINUE 

210  CONTINUE 
C 

C 

C    P(K/K-1)=PHI<K/K-1)*P<K-1/K-1)*TRANSP0SE<PHI )+Q( K-ll 

CALL    QUADPS<PHI,PKK,Q,6»6,PKK1} 
C 
C 
C    G(K)=P(K/K-1)*H<K)*INV(H<K)*P<K/K-1)*TRANSP0SE<H)+R<K) ) 

CALL    qUACPS(H,PKKl,R,6,6,EXTRAl) 

DO    212    I    =1,3 
00    211    J  =  l,3 

EXTRA4< I,J)=EXTRAHI,J) 

211  CONTINUE 

212  CONTINUE 

DO  230  1=1,6 

00  225  J=l,6 

EXTRA21 I,J)=0.0 
nr  p?n  k  si  /■» 

EXTPA2( I , J)=EXTRA2<I, J ) +PKK1 < I , K J*H< J,K) 

220         CCNTINUE 

225      CONTINUE 

230   CONTINUE 
C 
C 

c 

C  INVEFT  EXTRA1  AND  MULTIPLY  BY  EXTRA2. 

CALL    LINV2F<EXTRA4,3,3,EXTRA3,IDGT,WKAREA,IER) 
CALL    PRCDCK  EXTRA 2, EXTRA 3,3,3,6,0) 
GX(N)=G(1,1 
GY(M)=G<3,2) 
GZ(N)=G(5,3) 
C    X( K/K)=X( K/X-1)+G(K)*<Z(K)-H(K)*X(K/K-1 j ) 
DO    250    1=1,6 

EXTRAHI,l)=Zm 
00    240    J    =1,6 

EXTRAK  I,1)=EXTRA1(I,1)-H(  I,J)*XKK1(J) 
240  CONTINUE 

250       CONTINUE 

RADRES(N)  =  SQPT  (EXTRAK  1 ,  1)**2+EXTRA1  (  2, 1  )**2  + 
+  EXTR4H  3,1)**2> 
XRES(N)  =  EXTRAi<l,U 
YRES(N)=EXTRA1<2,1) 
ZRE5(N)=EXTRA1(3,1J 
00    270    1=1,6 

XKK(I  )  =  XKK1(I) 
DO    26C    J=l,3 

XKK(I)=XKK(I )+G<  I,J1*EXTRAKJ,1) 
260  CONTINUE 

270       CONTINUE 

TACREX(1,N)=XKK( 1) 
TAC^EX(2,N)=XKK<2) 
TAC^EY( 1,N)=XKK( 3) 
TACREY(2,NJ=XKK44) 
TAC?EZ( l,N)=XKK(5i 
TACREZ<2,N)=XKK<6) 
C 

C  P(K/K)=?<  K/K-l)-G(K)*H(KJ*P(K/K-li 
CALL  PRCDCT(G,h,6,6,6,EXTRAl) 
C&LL  ?RCDCT<EXTRAl,PKKl,b,6,6,EXTRA2) 
DO  290  1=1,6 

DO  280  J=l,6 
„  m  PKK(I,J)=PKK1(I,J)-EXTRA2(I,J) 

280      CONTINUE 
290   CONTINUE 
C 
999   CONTINUE 
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c 
c 
c 


c 

c 
c 


c 
c 

c 


P(N»1)=PKK<1,1) 
P(N,2)=PKK<2,2) 
P<N,3)=PKK<3,3) 
°<N,4-)  =  PKKU,4) 
P(N,5)=PKK<5,5) 
P(  N,6)=PKK(6,6) 

RETURN 
END 


SUBROUTINE    KALMN2< KAD,DT,RS IGMA , ASI GMA,ES  IGMA , RANGE, 
+AZMITH, ELEVTNtTTGJ 


DOUBLE    PRECISION    OSEED 

COMMON/OCUBLE/DSEED 

COMMON/ T*RRST /N, TACRRX < 3, 10GO ),TACRRY( 3, 1000) , 
+T4CRRZ(3,1000) 

COMMON/NCISE/TACRMX< 1000) , TACRMY (1000J ,TACRMZ ( 1000  J 

C0MM0N/FILTER/TACREX(3, 1000  J, TACR£Y< 3,1000), 
♦TACREZ(3,1000) 

C0MM0N/PRCC£S/X2SIGM,Y2SIGM,Z2S1GM,EWVRRX,EWVRRY, 
♦GX(IOOO) tGY(lOOO) , GZ (1000) , RADRES( 1000) ,XRES( 1000) , 
♦YRES(IOCO) ,ZP£S( 1000 ) , Will 1000  i ,W22( 1000) 
♦,W33(10C0),P< 1000,9) ,PINITL 

DIMENSION    Q(9,9) , PKK (9 ,9 ) , R( 9,9 J  ,W(9,9) ,G(9,9), 
+  PHI<<3,9) , DEL (9, 9), 01 (9, 9), NULL (9,9) , H< 9, 9 ) ,XKK( 9) , 
+  XKKK9)  ,FK1(9),EXTRA1(9,9)  ,  EXTRA2(  9,  9  ) ,  PK,K1  (  9  ,9) 
+,WKAPSA(75),EXTRA3<3,3) ,EXTRA4<3,3) ,Z(9) 


IF 

(N    .NE.     1     )    GO    TC 

i    25 

IDGT=2 

DT, 

2=OT*DT/2.0 

0T3=OT2*CT/3.0 

DO 

20    1=1,9 

Z(I)=0.0 

DO    10    J=l,9 
Q(I,J)=C.O 
PKK( I,J)=0.0 

IF    (I    .EG. 

J) 

R(  I,J)  =  0.0 

wu,  j)  =  c.o 

G(!t J)=C.O 

PHK  I,  J  )=0.0 

DEL( I,J)=0.0 

DI(I,J)=0.0 
IF    (I    .EG. 

J) 

NULLd  ,  J)  =  0.0 

H(I,J)«0.0 

10 

CONTINUE 

20         CQI 

MTINUE 

UL< 

=X2SIG^*X2SIGM 

W2^ 

=Y2SIGM*Y2SIGM 

W3 

=Z2SIGM*Z2SIGM 

hX^ 

=  0.0 

WY 

=WX 

wz= 

=  WY 

IXFLG=0 

IYFLG=0 

PKK(I,J)=PINITL 


CI(  It Jl«l  .0 
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c 

r 
C 

c 
c 
c 


IZFLG=0 

DEU1,1)=DT3 

DEL(2,1)=DT2 

DEL<3,1)=DT 

CEU4,2)=DT3 

DEL(5,2)=DT2 

DEL(6,2)=DT 

DEL(7,3)=DT3 

DEL(8,3)=DT2 

DEL(9,3)=0T 

H(  1,1)=1.0 

H<2,4)=1.0 

H(  3,7)-1.0 

PHI(1,1)=1.0 

PHI(1,2)=DT 

PHI(1,3)=DT2 

PHI(2,2)=1.0 

PHK  2,3)=DT 

PHI(3,3)  =  1.0 

PHH<n4)=1.0 

PHH4,5)=DT 

PHI(4,6)=DT2 

PHH5,5)=1.C 

PHT(5,6I=0T 

PHI (6,6) =1.0 

PHI<7,7)=1.0 

PHI(7,8)=DT 

PHI(7,9)=DT2 

PHI<8,8)=1.0 

PHI(8,9)=DT 

PHI  (9, 9)  =1.0 

PVAR=RSIGMA*RSIGMA 

AVAR=ASIGMA*ASIGMA 

EVAR=ESIGMA*ESIGMA 

TACREX(1,N)=TACRMX(1) 

XKK(1)=TACREX(1,N) 

TACREXC 2fN)=TACRRX(2tNJ+EWVRRX 

XKK(2)=TACPEX(2,NJ 

XKK(3)=TACRRZ<3,N) 

TACREY(1,N)=TACRMY(1J 

XKKm=TACREY<l,N) 

TACREY(2tN)=TACRRY(2,NJ+EWVRRY 

XKK(5)=TACREY(2,N) 

XKK<6)=TACRRY(3,N) 

TACREZ<1»N)=TACRMZ(1) 

XKK(7)=TACREZ{1,N) 

TACREZ( 2,N)=TACRRZ(2,N) 

XKK(8)=TACPEZ{2,N) 

XKK(9)=TACRRZ(3,N) 

IF    (N    .EQ.     1)    GG    TO    999 
CONTINUE 


Z(1)=TACPMX(N) 
Z(2)=TACRMY(N) 
Z(3)=TACRMZ<N) 


R2=RANGE*RANGE 
CA=CCS( AZMITHi 
SA=SIN(AZMITH) 
CE=CHS(ELEVTN) 
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SE=SIN(ELEVTN) 

CA2-CA*CA 

SA2=SA*SA 

CE2=CE*CE 

SE2=SE*SE 
C 
C 

R<1,1)=R2*<EVAR*SE2*SA2+AVAR*CE2*CA2)+RVAR*CE2*SA2 

R(2t2)=P2*(EVAR*SE2*CA2+AVAR*CE2*SA2)+RVAR*CE2*CA2 

R( 3,3)=R2*EVAR*CE2+RVAR*SE2 

R( l,2)=R2*EVAR*SE2*SA*CA+< RVAR-R2*AVAR )*( CE2*SA*CA) 

R(2,1)=R(1,2) 

R< l,3)=(RyAR-R2*EVAR)*SE*CE*SA 

R(3,1)=R<1.3) 

R( 2,3)=<RVAR-P2*EVAR)*SE*CE*CA 

R(3,2)=R(2,3) 


c 

c 

c 

FKl(l)*C.O 

FK1(2)  =  0.0 
FK1(3)=0.0 
.  FK1<4)=0.0 
FK1(5)=0.0 
FKH6)=0.0 
FK1(7)  =  0.0 
FK1(8I=0.0 
FK1(9>=0.0 

c 

c 

IF     (KAD    .NE. 

0)    GQ 

W(1,1)=W1 

W(2,2)=W2 

W(3,3)=W3 

GO    TC    34 

29 

CONTINUE 

IF     (KAD     .NE. 

1)    GQ 

CALL    ADPTV1(N,9, 

TC    29 


TO    31 
120,Wl,W2,W3,TACaSXfTACREY,TACREZ, 

GO  TC  34 
31    CONTINUE 

IF    (TTG    .LT.    20.)    GO    TO    33 
XRS=XRES<N-1) 
YRS=YPES(N-1) 
ZPS=ZRES<N-1) 
CALL    A0PTV2<9,XRS,IXFLG,YRS,IYFLG,ZRS,IZFLG,PKK) 

33  CONTINUE 
W( 1,1)=W1 
W(2,2)=W2 
W(3,3)=W3 

34  CONTINUE 
W11(N)  =  VN1,1) 
W22(N)=W(2,2) 
W33(N)=V|(  3,3) 

C 

C    GENERATE    O-MATRIX. 

CALL    QUADPS(DEL,*,NULL,9,9,Q) 

C 

C    X<K/K-l)=PHI(KtK-l)*X(K-l/K-l)+F(K-l) 
DO    210    1=1,9 

XKKHI)=FK1(I) 
00    2CC    J=l,9 

XKKK I)=XKK1(I)+PHI( I, J)*XKK( J) 
200  CONTINUE 

210       CONTINUE 
C 
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c 

C  P(K/K-1I  =  PHI (K/K-1)*P(K-1/K-1)*TRANS POSE (PHI )+Q(K-lJ 

CALL  QUA0PS(PHI,PKK,Q,9,9,PKK1) 
C 
C 

C  G(K)=P(K/K-l)*H<K)*INV(H<K)*P<K/K-l)*TRANSPOSE<H}+R(Kn 
CALL  QUADPS(H,PKK1,R,9,9,EXTRA1) 
DO  212  I  =1,3 
DO  211  J  =  l,3 

EXTRA4<  I,JJ=EXTRA1(I,J) 

211  CONTINUE 

212  CONTINUE 

DO  230  1=1,9 
00  225  J  =  l  ,9 

EXTRA2( I,J)=0.0 
DC  220  K=l,9 

EXTRA2(I,J)=EXTRA2(I, J ) +PKK1 < I , KJ*H < J,KJ 
220         CONTINUE 
225      CONTINUE 
230   CONTINUE 
C 
C 
C 
C  INVEST  EXTRA1  AND  MULTIPLY  BY  EXTRA2. 

CALL  LI  NV2F(EXTRA*,3,3,EXTRA3, I DGT, WKAREA, IER ) 

CALL  ?RC0CT(EXTRA2,EXTRA3,3,3,9,G) 
GX(N)=G(1,1) 
GY(N)=G(4t2) 
GZ(N)=G(7,3) 
C  X<K/K)=X<K/K-1)+G(K)*(Z(K)-F(K)*X<K/K-1J  )  - 
DO  250  1=1,9 

EXTRAH  I,1)=Z(I) 
DO  240  J  =1,9 

EXTRA 1( I,1)=EXTRA1(I ,  1 J-ri < I , J ) *XKK1 ( J ) 
240      CONTINUE 
250   CONTINUE 

RADRES(N)=SQRT(EXTRA1(1,1)**2+EXTRA1(2,1)**2+ 
+EXTRA1<3,1)**2) 
XRES<N)=EXTRA1(1,1) 
YRES(N)=EXTRA1(2,1> 
ZRES(N)=EXTRA1(3,1) 
DO    270    1=1,9 

XKK(  I  )  =  XKK1(I  ) 
DO    260    J=l,3 

XKK(I)«XKK(Ii+G(I  ,  JJ*EXTRA1( J,l) 
260  CONTINUE 

270      CONTINUE 

TACREX<  1,N)=XKK(1) 
TACREX(2,N)=XKK(2) 
TACR£X( 3,N)=XKK(3) 
TACREYt 1,N)=XKK(4) 
TACREYC  2,N)=XKK(5J 
TACREY(3,N)=XKK<6) 
TACREZt 1,N)=XKK<7) 
TACREZ(2,N)=XKK<8) 
TACREZ( 3,N)=XKK(9J 
C 
C 

C    P< K/K)=P(K/K-1)-G(K)*H(KJ*P(K/K-1) 
CALL    PRCCCT(G,H,9,9,9,EXTRA1 J 
CALL    PR0CCT<EXTRA1,PKK1,9,9,9,EXTRA2) 
DO    290    1=1,9 

DO  2  8C  J  =  l,9 

PKK(I, J)=PKK1( I, JJ-EXTRA2(I,J) 
280      CONTINUE 
290   CONTINUE 
999   CONTINUE 
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P(Nfl)«PKK<L,lJ 

P(N,2)=PKK(2,2) 

P<N,3)=PKK<3,3) 

P(N,4)=PKK<4,4) 

P(N,5)=PKK(5,5) 

P<N,6)=PKK<6,6) 

P(N,7)=PKK(7,7) 

P(N,8)=PKK<8,8) 

P<N,9|=PKK(9,9) 
C 
C15    FORMAT  (•  •.•GCIlf*  t«  t  lit'  >=»,F20.10) 


C 
C 
C 


RETURN 
END 


C 

c 
c 


SUBROUTINE  GUAOPS ( X , H, R ,M, N ,C ) 

01  PENSION  X<M,N),H(N,N),R(MfM),C(MfM) 
DO  400  I  =  1,M 
DO  300  J=ltM 

C(I,  JJsRdtJJ 
DC  200  K=1,N 

DO  IOC  L  =  1,N 

C(I,  J)=C(  I»J)+X(I,KJ*H(K,L)*X<J,U 
100  CONTINUE 

200  CCNTINUE 

300  CONTINUE 

400         CONTINUE 
RETURN 
END 


C 

c 
c 


SUBROUTINE    ADPTVKN,  13  ,  JSTART , Wl ,W2 , W3,TACFEX , TACREY , 
+  TACREZ,Vj) 

DI  PENSION    TACREXOtlOOO)  ,  TACREY(  3,  1000  )♦ 
♦TACREK  3*1000  )fWU3»  131 
ISTATE=I3/3 
IF    (N    .LT.     JSTART)    GO    TO    27 

WX=8.0*(TACREX(ISTATE,N-l)-TACREX(ISTATEfN-2) ) 
WY=8.C*<TACREY<ISTATE,N-1)-TACREY<ISTATE,N-2J ) 
W Z=8. C* < TACREZ U STATE, N-1)-TACREZ( I  STAT E,N-2)i 
GC    TO    35 
27  CONTINUE 

WX=0.0 
WY=0.0 
WZ=C.O 
35  CONTINUE 

IF    (  ISTATE    .EQ.    3)    GC    TO    37 
W(l,l)=Wl>(WX*WXJ/3. 
W(2,2)=W2+(WY*WY)/3. 
W<3,3)=w3-MWZ*WZ)/3. 
GO    TC    41 
37  CONTINUE 

W(  1,  U  =  Wl+(  WX*WX)/81. 
W<2,2)  =  V»2+<WY*WY)/81. 
H  (3,3)=W3+(kZ*WZ)/31. 
41  CONTINUE 

RETURN 
END 


SUBROUTINE    ADPTV2 (NGRDER ,XRES, IXFLG, YRES»  IYFLG»ZRES» 
+IZCLG,PKK) 

DIMENSION  PKK(NCRDER,NORDERJ 
RESET=1000. 
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CftLL    BIAS(XPES,IXFLG) 
IF    HABSUXFLG)     .LT.    8)    GO    TO    10 
PKK<1,1)=RESET 
PKK<2,2)=RESET 

IF    (NCRCER    .5Q.    9)    PKK( 3, 3 )=RESET 
10  CONTINUE 

CALL  8IAS(YRES,IYFLGJ 
IF  (IABS(IYFLG)  .LT.  8)  GO  TO  14 
IF  (NORCER  .EQ.  9i  GO  TO  12 
PKK(2t3)-RESET 
PKK<4,4)=RESET 
GO  TO  14 
12  CONTINUE 

PKK<4,4)=RESET 

PKK(5,5)=RESET 
PKK(6,6)=RESET 
14      CONTINUE 

CALL    BIAS<ZRES,IZFLG) 
IF    (IA8S(IZFLG)     .LT.    8J    GO    TO    20 
IF    (NORDER    .EQ.    9)    GO    TO    16 
PKK(5,5)=RESET 
PKM6,6)  =  RESET 
GO   TC    20 
16  CONTINUE 

PKK(7,7)=RESET 
PKK( 8,8J=RESET 
PKK(9,9)=RESET 
20  CONTINUE 

RETURN 
END 


c 
c 

SUBROU 

IP 

(RESIDU)  10,20,30 

10 

IF  (IFLAG  .LE.  0)  GO  TO 
IFLAG=0 
GO  TG  40 

12 

12 

CONTINUE 

IFLAG=IFLAG-1 

14 

GO  TC  40 

20 

If=LAG=0 
GO  TO  40 

30 

Ic  (IFLAG  .GE.  OJ  GO  TO 
IFLAG=0 

GO  TC  40 

32 

32 

CONTINUE 
I*=LAG  =  IFLAG  +  1 

40 

C0N1 

RE 

TURN 

END 
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